diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
new file mode 100644
index 0000000..25d674f
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
@@ -0,0 +1,874 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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-2020 Analog Devices Inc. All Rights Reserved.           */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*                                                                            */
+/* Modified by Juan Chong - frcsupport@analog.com                             */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ADIS16448_IMU.h"
+
+#include <frc/DigitalInput.h>
+#include <frc/DigitalOutput.h>
+#include <frc/DigitalSource.h>
+#include <frc/DriverStation.h>
+#include <frc/SPI.h>
+#include <frc/Timer.h>
+
+#include <algorithm>
+#include <cmath>
+#include <string>
+
+#include <hal/HAL.h>
+#include <networktables/NTSendableBuilder.h>
+#include <wpi/numbers>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+
+/* Helpful conversion functions */
+static inline uint16_t BuffToUShort(const uint32_t* buf) {
+  return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
+}
+
+static inline uint8_t BuffToUByte(const uint32_t* buf) {
+  return static_cast<uint8_t>(buf[0]);
+}
+
+static inline int16_t BuffToShort(const uint32_t* buf) {
+  return (static_cast<int16_t>(buf[0]) << 8) | buf[1];
+}
+
+static inline uint16_t ToUShort(const uint8_t* buf) {
+  return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
+}
+
+using namespace frc;
+
+namespace {
+template <typename S, typename... Args>
+inline void ADISReportError(int32_t status, const char* file, int line,
+                            const char* function, const S& format,
+                            Args&&... args) {
+  frc::ReportErrorV(status, file, line, function, format,
+                    fmt::make_args_checked<Args...>(format, args...));
+}
+}  // namespace
+
+#define REPORT_WARNING(msg) \
+  ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
+#define REPORT_ERROR(msg) \
+  ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
+
+ADIS16448_IMU::ADIS16448_IMU()
+    : ADIS16448_IMU(kZ, SPI::Port::kMXP, CalibrationTime::_512ms) {}
+
+ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
+                             CalibrationTime cal_time)
+    : m_yaw_axis(yaw_axis),
+      m_spi_port(port),
+      m_simDevice("Gyro:ADIS16448", port) {
+  if (m_simDevice) {
+    m_simGyroAngleX =
+        m_simDevice.CreateDouble("gyro_angle_x", hal::SimDevice::kInput, 0.0);
+    m_simGyroAngleY =
+        m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0);
+    m_simGyroAngleZ =
+        m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0);
+    m_simGyroRateX =
+        m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
+    m_simGyroRateY =
+        m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
+    m_simGyroRateZ =
+        m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
+    m_simAccelX =
+        m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
+    m_simAccelY =
+        m_simDevice.CreateDouble("accel_y", hal::SimDevice::kInput, 0.0);
+    m_simAccelZ =
+        m_simDevice.CreateDouble("accel_z", hal::SimDevice::kInput, 0.0);
+  }
+
+  if (!m_simDevice) {
+    // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+    // Relies on the RIO hardware by default configuring an output as low
+    // and configuring an input as high Z. The 10k pull-up resistor internal to
+    // the IMU then forces the reset line high for normal operation.
+    DigitalOutput* m_reset_out = new DigitalOutput(18);  // Drive MXP DIO8 low
+    Wait(10_ms);                                         // Wait 10ms
+    delete m_reset_out;
+    new DigitalInput(18);  // Set MXP DIO8 high
+    Wait(500_ms);          // Wait 500ms for reset to complete
+
+    ConfigCalTime(cal_time);
+
+    // Configure standard SPI
+    if (!SwitchToStandardSPI()) {
+      return;
+    }
+
+    // Set IMU internal decimation to 819.2 SPS
+    WriteRegister(SMPL_PRD, 0x0001);
+    // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
+    WriteRegister(MSC_CTRL, 0x0016);
+    // Disable IMU internal Bartlett filter
+    WriteRegister(SENS_AVG, 0x0400);
+    // Clear offset registers
+    WriteRegister(XGYRO_OFF, 0x0000);
+    WriteRegister(YGYRO_OFF, 0x0000);
+    WriteRegister(ZGYRO_OFF, 0x0000);
+    // Configure and enable auto SPI
+    if (!SwitchToAutoSPI()) {
+      return;
+    }
+    // Notify DS that IMU calibration delay is active
+    REPORT_WARNING(
+        "ADIS16448 IMU Detected. Starting initial calibration delay.");
+    // Wait for whatever time the user set as the start-up delay
+    Wait(static_cast<double>(m_calibration_time) * 1.2_s);
+    // Execute calibration routine
+    Calibrate();
+    // Reset accumulated offsets
+    Reset();
+    // Tell the acquire loop that we're done starting up
+    m_start_up_mode = false;
+
+    // Let the user know the IMU was initiallized successfully
+    REPORT_WARNING("ADIS16448 IMU Successfully Initialized!");
+
+    // TODO: Find what the proper pin is to turn this LED
+    //  Drive MXP PWM5 (IMU ready LED) low (active low)
+    new DigitalOutput(19);
+  }
+
+  // Report usage and post data to DS
+  HAL_Report(HALUsageReporting::kResourceType_ADIS16448, 0);
+
+  wpi::SendableRegistry::AddLW(this, "ADIS16448", port);
+}
+
+/**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in standard SPI mode.
+ *
+ * This function switches the active SPI port to standard SPI and is used
+ *primarily when exiting auto SPI. Exiting auto SPI is required to read or write
+ *using SPI since the auto SPI configuration, once active, locks the SPI message
+ *being transacted. This function also verifies that the SPI port is operating
+ *in standard SPI mode by reading back the IMU product ID.
+ **/
+bool ADIS16448_IMU::SwitchToStandardSPI() {
+  // Check to see whether the acquire thread is active. If so, wait for it to
+  // stop producing data.
+  if (m_thread_active) {
+    m_thread_active = false;
+    while (!m_thread_idle) {
+      Wait(10_ms);
+    }
+    // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
+    if (m_spi != nullptr && m_auto_configured) {
+      m_spi->StopAuto();
+      // We need to get rid of all the garbage left in the auto SPI buffer after
+      // stopping it. Sometimes data magically reappears, so we have to check
+      // the buffer size a couple of times
+      //  to be sure we got it all. Yuck.
+      uint32_t trashBuffer[200];
+      Wait(100_ms);
+      int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+      while (data_count > 0) {
+        /* Dequeue 200 at a time, or the remainder of the buffer if less than
+         * 200 */
+        m_spi->ReadAutoReceivedData(trashBuffer, std::min(200, data_count),
+                                    0_s);
+        /* Update remaining buffer count */
+        data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+      }
+    }
+  }
+  // There doesn't seem to be a SPI port active. Let's try to set one up
+  if (m_spi == nullptr) {
+    m_spi = new SPI(m_spi_port);
+    m_spi->SetClockRate(1000000);
+    m_spi->SetMSBFirst();
+    m_spi->SetSampleDataOnTrailingEdge();
+    m_spi->SetClockActiveLow();
+    m_spi->SetChipSelectActiveLow();
+    ReadRegister(PROD_ID);  // Dummy read
+
+    // Validate the product ID
+    uint16_t prod_id = ReadRegister(PROD_ID);
+    if (prod_id != 16448) {
+      REPORT_ERROR("Could not find ADIS16448!");
+      Close();
+      return false;
+    }
+    return true;
+  } else {
+    // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
+    // product ID.
+    ReadRegister(PROD_ID);  // Dummy read
+    uint16_t prod_id = ReadRegister(PROD_ID);
+    if (prod_id != 16448) {
+      REPORT_ERROR("Could not find ADIS16448!");
+      Close();
+      return false;
+    } else {
+      return true;
+    }
+  }
+}
+
+void ADIS16448_IMU::InitOffsetBuffer(int size) {
+  // avoid exceptions in the case of bad arguments
+  if (size < 1) {
+    size = 1;
+  }
+
+  // set average size to size (correct bad values)
+  m_avg_size = size;
+
+  // resize vector
+  if (m_offset_buffer != nullptr) {
+    delete[] m_offset_buffer;
+  }
+  m_offset_buffer = new OffsetData[size];
+
+  // set accumulate count to 0
+  m_accum_count = 0;
+}
+
+/**
+ * This function switches the active SPI port to auto SPI and is used primarily
+ *when exiting standard SPI. Auto SPI is required to asynchronously read data
+ *over SPI as it utilizes special FPGA hardware to react to an external data
+ *ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
+ *can be read by the CPU asynchronously. Standard SPI transactions are
+ * impossible on the selected SPI port once auto SPI is enabled. The stall
+ *settings, GPIO interrupt pin, and data packet settings used in this function
+ *are hard-coded to work only with the ADIS16448 IMU.
+ **/
+bool ADIS16448_IMU::SwitchToAutoSPI() {
+  // No SPI port has been set up. Go set one up first.
+  if (m_spi == nullptr) {
+    if (!SwitchToStandardSPI()) {
+      REPORT_ERROR("Failed to start/restart auto SPI");
+      return false;
+    }
+  }
+  // Only set up the interrupt if needed.
+  if (m_auto_interrupt == nullptr) {
+    m_auto_interrupt = new DigitalInput(10);
+  }
+  // The auto SPI controller gets angry if you try to set up two instances on
+  // one bus.
+  if (!m_auto_configured) {
+    m_spi->InitAuto(8200);
+    m_auto_configured = true;
+  }
+  // Set auto SPI packet data and size
+  m_spi->SetAutoTransmitData({{GLOB_CMD}}, 27);
+  // Configure auto stall time
+  m_spi->ConfigureAutoStall(HAL_SPI_kMXP, 100, 1000, 255);
+  // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
+  // activated)
+  m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
+  // Check to see if the acquire thread is running. If not, kick one off.
+  if (!m_thread_idle) {
+    m_first_run = true;
+    m_thread_active = true;
+    // Set up circular buffer
+    InitOffsetBuffer(m_avg_size);
+    // Kick off acquire thread
+    m_acquire_task = std::thread(&ADIS16448_IMU::Acquire, this);
+  } else {
+    m_first_run = true;
+    m_thread_active = true;
+  }
+  // Looks like the thread didn't start for some reason. Abort.
+  /*
+  if(!m_thread_idle) {
+    REPORT_ERROR("Failed to start/restart the acquire() thread.");
+    Close();
+    return false;
+  }
+  */
+  return true;
+}
+
+/**
+ *
+ **/
+int ADIS16448_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
+  if (m_calibration_time == new_cal_time) {
+    return 1;
+  } else {
+    m_calibration_time = new_cal_time;
+    m_avg_size = static_cast<uint16_t>(m_calibration_time) * 819;
+    InitOffsetBuffer(m_avg_size);
+    return 0;
+  }
+}
+
+/**
+ *
+ **/
+void ADIS16448_IMU::Calibrate() {
+  std::scoped_lock sync(m_mutex);
+  // Calculate the running average
+  int gyroAverageSize = std::min(m_accum_count, m_avg_size);
+  double accum_gyro_rate_x = 0.0;
+  double accum_gyro_rate_y = 0.0;
+  double accum_gyro_rate_z = 0.0;
+  for (int i = 0; i < gyroAverageSize; i++) {
+    accum_gyro_rate_x += m_offset_buffer[i].gyro_rate_x;
+    accum_gyro_rate_y += m_offset_buffer[i].gyro_rate_y;
+    accum_gyro_rate_z += m_offset_buffer[i].gyro_rate_z;
+  }
+  m_gyro_rate_offset_x = accum_gyro_rate_x / gyroAverageSize;
+  m_gyro_rate_offset_y = accum_gyro_rate_y / gyroAverageSize;
+  m_gyro_rate_offset_z = accum_gyro_rate_z / gyroAverageSize;
+  m_integ_gyro_angle_x = 0.0;
+  m_integ_gyro_angle_y = 0.0;
+  m_integ_gyro_angle_z = 0.0;
+}
+
+/**
+ * This function reads the contents of an 8-bit register location by
+ *transmitting the register location byte along with a null (0x00) byte using
+ *the standard WPILib API. The response (two bytes) is read back using the
+ *WPILib API and joined using a helper function. This function assumes the
+ *controller is set to standard SPI mode.
+ **/
+uint16_t ADIS16448_IMU::ReadRegister(uint8_t reg) {
+  uint8_t buf[2];
+  buf[0] = reg & 0x7f;
+  buf[1] = 0;
+
+  m_spi->Write(buf, 2);
+  m_spi->Read(false, buf, 2);
+
+  return ToUShort(buf);
+}
+
+/**
+ * This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
+ *via SPI. The upper and lower bytes that make up the 16-bit value are split
+ *into two unsined, 8-bit values and written to the upper and lower addresses of
+ *the specified register value. Only the lower (base) address must be specified.
+ *This function assumes the controller is set to standard SPI mode.
+ **/
+void ADIS16448_IMU::WriteRegister(uint8_t reg, uint16_t val) {
+  uint8_t buf[2];
+  buf[0] = 0x80 | reg;
+  buf[1] = val & 0xff;
+  m_spi->Write(buf, 2);
+  buf[0] = 0x81 | reg;
+  buf[1] = val >> 8;
+  m_spi->Write(buf, 2);
+}
+
+/**
+ * This function resets (zeros) the accumulated (integrated) angle estimates for
+ *the xgyro, ygyro, and zgyro outputs.
+ **/
+void ADIS16448_IMU::Reset() {
+  std::scoped_lock sync(m_mutex);
+  m_integ_gyro_angle_x = 0.0;
+  m_integ_gyro_angle_y = 0.0;
+  m_integ_gyro_angle_z = 0.0;
+}
+
+void ADIS16448_IMU::Close() {
+  if (m_thread_active) {
+    m_thread_active = false;
+    if (m_acquire_task.joinable()) {
+      m_acquire_task.join();
+    }
+  }
+  if (m_spi != nullptr) {
+    if (m_auto_configured) {
+      m_spi->StopAuto();
+    }
+    delete m_spi;
+    m_auto_configured = false;
+    if (m_auto_interrupt != nullptr) {
+      delete m_auto_interrupt;
+      m_auto_interrupt = nullptr;
+    }
+    m_spi = nullptr;
+  }
+  delete[] m_offset_buffer;
+}
+
+ADIS16448_IMU::~ADIS16448_IMU() {
+  Close();
+}
+
+void ADIS16448_IMU::Acquire() {
+  // Set data packet length
+  const int dataset_len = 29;  // 18 data points + timestamp
+
+  const int BUFFER_SIZE = 4000;
+
+  // This buffer can contain many datasets
+  uint32_t buffer[BUFFER_SIZE];
+  int data_count = 0;
+  int data_remainder = 0;
+  int data_to_read = 0;
+  int bufferAvgIndex = 0;
+  uint32_t previous_timestamp = 0;
+  double gyro_rate_x = 0.0;
+  double gyro_rate_y = 0.0;
+  double gyro_rate_z = 0.0;
+  double accel_x = 0.0;
+  double accel_y = 0.0;
+  double accel_z = 0.0;
+  double mag_x = 0.0;
+  double mag_y = 0.0;
+  double mag_z = 0.0;
+  double baro = 0.0;
+  double temp = 0.0;
+  double gyro_rate_x_si = 0.0;
+  double gyro_rate_y_si = 0.0;
+  // double gyro_rate_z_si = 0.0;
+  double accel_x_si = 0.0;
+  double accel_y_si = 0.0;
+  double accel_z_si = 0.0;
+  double compAngleX = 0.0;
+  double compAngleY = 0.0;
+  double accelAngleX = 0.0;
+  double accelAngleY = 0.0;
+
+  while (true) {
+    // Sleep loop for 10ms (wait for data)
+    Wait(10_ms);
+
+    if (m_thread_active) {
+      data_count = m_spi->ReadAutoReceivedData(
+          buffer, 0,
+          0_s);  // Read number of bytes currently stored in the buffer
+      data_remainder =
+          data_count % dataset_len;  // Check if frame is incomplete
+      data_to_read = data_count -
+                     data_remainder;  // Remove incomplete data from read count
+      /* Want to cap the data to read in a single read at the buffer size */
+      if (data_to_read > BUFFER_SIZE) {
+        REPORT_WARNING(
+            "ADIS16448 data processing thread overrun has occurred!");
+        data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+      }
+      m_spi->ReadAutoReceivedData(buffer, data_to_read,
+                                  0_s);  // Read data from DMA buffer
+
+      // Could be multiple data sets in the buffer. Handle each one.
+      for (int i = 0; i < data_to_read; i += dataset_len) {
+        // Calculate CRC-16 on each data packet
+        uint16_t calc_crc = 0xFFFF;  // Starting word
+        uint8_t byte = 0;
+        uint16_t imu_crc = 0;
+        for (int k = 5; k < 27;
+             k += 2)  // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP
+                      // (Ignore Status & CRC)
+        {
+          byte = BuffToUByte(&buffer[i + k + 1]);  // Process LSB
+          calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
+          byte = BuffToUByte(&buffer[i + k]);  // Process MSB
+          calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
+        }
+        calc_crc = ~calc_crc;  // Complement
+        calc_crc = static_cast<uint16_t>((calc_crc << 8) |
+                                         (calc_crc >> 8));  // Flip LSB & MSB
+        imu_crc =
+            BuffToUShort(&buffer[i + 27]);  // Extract DUT CRC from data buffer
+
+        // Compare calculated vs read CRC. Don't update outputs or dt if CRC-16
+        // is bad
+        if (calc_crc == imu_crc) {
+          // Timestamp is at buffer[i]
+          m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
+          // Split array and scale data
+          gyro_rate_x = BuffToShort(&buffer[i + 5]) * 0.04;
+          gyro_rate_y = BuffToShort(&buffer[i + 7]) * 0.04;
+          gyro_rate_z = BuffToShort(&buffer[i + 9]) * 0.04;
+          accel_x = BuffToShort(&buffer[i + 11]) * 0.833;
+          accel_y = BuffToShort(&buffer[i + 13]) * 0.833;
+          accel_z = BuffToShort(&buffer[i + 15]) * 0.833;
+          mag_x = BuffToShort(&buffer[i + 17]) * 0.1429;
+          mag_y = BuffToShort(&buffer[i + 19]) * 0.1429;
+          mag_z = BuffToShort(&buffer[i + 21]) * 0.1429;
+          baro = BuffToShort(&buffer[i + 23]) * 0.02;
+          temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0;
+
+          // Convert scaled sensor data to SI units
+          gyro_rate_x_si = gyro_rate_x * deg_to_rad;
+          gyro_rate_y_si = gyro_rate_y * deg_to_rad;
+          // gyro_rate_z_si = gyro_rate_z * deg_to_rad;
+          accel_x_si = accel_x * grav;
+          accel_y_si = accel_y * grav;
+          accel_z_si = accel_z * grav;
+          // Store timestamp for next iteration
+          previous_timestamp = buffer[i];
+          // Calculate alpha for use with the complementary filter
+          m_alpha = m_tau / (m_tau + m_dt);
+          // Calculate complementary filter
+          if (m_first_run) {
+            accelAngleX = atan2f(
+                -accel_x_si,
+                sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+            accelAngleY =
+                atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
+                                         (-accel_z_si * -accel_z_si)));
+            compAngleX = accelAngleX;
+            compAngleY = accelAngleY;
+          } else {
+            accelAngleX = atan2f(
+                -accel_x_si,
+                sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+            accelAngleY =
+                atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
+                                         (-accel_z_si * -accel_z_si)));
+            accelAngleX = FormatAccelRange(accelAngleX, -accel_z_si);
+            accelAngleY = FormatAccelRange(accelAngleY, -accel_z_si);
+            compAngleX =
+                CompFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
+            compAngleY =
+                CompFilterProcess(compAngleY, accelAngleY, -gyro_rate_x_si);
+          }
+
+          // Update global variables and state
+          {
+            std::scoped_lock sync(m_mutex);
+            // Ignore first, integrated sample
+            if (m_first_run) {
+              m_integ_gyro_angle_x = 0.0;
+              m_integ_gyro_angle_y = 0.0;
+              m_integ_gyro_angle_z = 0.0;
+            } else {
+              // Accumulate gyro for offset calibration
+              // Add most recent sample data to buffer
+              bufferAvgIndex = m_accum_count % m_avg_size;
+              m_offset_buffer[bufferAvgIndex] =
+                  OffsetData{gyro_rate_x, gyro_rate_y, gyro_rate_z};
+              // Increment counter
+              m_accum_count++;
+            }
+            // Don't post accumulated data to the global variables until an
+            // initial gyro offset has been calculated
+            if (!m_start_up_mode) {
+              m_gyro_rate_x = gyro_rate_x;
+              m_gyro_rate_y = gyro_rate_y;
+              m_gyro_rate_z = gyro_rate_z;
+              m_accel_x = accel_x;
+              m_accel_y = accel_y;
+              m_accel_z = accel_z;
+              m_mag_x = mag_x;
+              m_mag_y = mag_y;
+              m_mag_z = mag_z;
+              m_baro = baro;
+              m_temp = temp;
+              m_compAngleX = compAngleX * rad_to_deg;
+              m_compAngleY = compAngleY * rad_to_deg;
+              m_accelAngleX = accelAngleX * rad_to_deg;
+              m_accelAngleY = accelAngleY * rad_to_deg;
+              // Accumulate gyro for angle integration and publish to global
+              // variables
+              m_integ_gyro_angle_x +=
+                  (gyro_rate_x - m_gyro_rate_offset_x) * m_dt;
+              m_integ_gyro_angle_y +=
+                  (gyro_rate_y - m_gyro_rate_offset_y) * m_dt;
+              m_integ_gyro_angle_z +=
+                  (gyro_rate_z - m_gyro_rate_offset_z) * m_dt;
+            }
+          }
+          m_first_run = false;
+        }
+      }
+    } else {
+      m_thread_idle = true;
+      data_count = 0;
+      data_remainder = 0;
+      data_to_read = 0;
+      previous_timestamp = 0.0;
+      gyro_rate_x = 0.0;
+      gyro_rate_y = 0.0;
+      gyro_rate_z = 0.0;
+      accel_x = 0.0;
+      accel_y = 0.0;
+      accel_z = 0.0;
+      mag_x = 0.0;
+      mag_y = 0.0;
+      mag_z = 0.0;
+      baro = 0.0;
+      temp = 0.0;
+      gyro_rate_x_si = 0.0;
+      gyro_rate_y_si = 0.0;
+      // gyro_rate_z_si = 0.0;
+      accel_x_si = 0.0;
+      accel_y_si = 0.0;
+      accel_z_si = 0.0;
+      compAngleX = 0.0;
+      compAngleY = 0.0;
+      accelAngleX = 0.0;
+      accelAngleY = 0.0;
+    }
+  }
+}
+
+/* Complementary filter functions */
+double ADIS16448_IMU::FormatFastConverge(double compAngle, double accAngle) {
+  if (compAngle > accAngle + wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  } else if (accAngle > compAngle + wpi::numbers::pi) {
+    compAngle = compAngle + 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+double ADIS16448_IMU::FormatRange0to2PI(double compAngle) {
+  while (compAngle >= 2 * wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  }
+  while (compAngle < 0.0) {
+    compAngle = compAngle + 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+double ADIS16448_IMU::FormatAccelRange(double accelAngle, double accelZ) {
+  if (accelZ < 0.0) {
+    accelAngle = wpi::numbers::pi - accelAngle;
+  } else if (accelZ > 0.0 && accelAngle < 0.0) {
+    accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
+  }
+  return accelAngle;
+}
+
+double ADIS16448_IMU::CompFilterProcess(double compAngle, double accelAngle,
+                                        double omega) {
+  compAngle = FormatFastConverge(compAngle, accelAngle);
+  compAngle =
+      m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+  compAngle = FormatRange0to2PI(compAngle);
+  if (compAngle > wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+int ADIS16448_IMU::ConfigDecRate(uint16_t DecimationSetting) {
+  uint16_t writeValue = DecimationSetting;
+  uint16_t readbackValue;
+  if (!SwitchToStandardSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+    return 2;
+  }
+
+  /* Check max */
+  if (DecimationSetting > 9) {
+    REPORT_ERROR("Attemted to write an invalid decimation value. Capping at 9");
+    DecimationSetting = 9;
+  }
+
+  /* Shift decimation setting to correct position and select internal sync */
+  writeValue = (DecimationSetting << 8) | 0x1;
+
+  /* Apply to IMU */
+  WriteRegister(SMPL_PRD, writeValue);
+
+  /* Perform read back to verify write */
+  readbackValue = ReadRegister(SMPL_PRD);
+
+  /* Throw error for invalid write */
+  if (readbackValue != writeValue) {
+    REPORT_ERROR("ADIS16448 SMPL_PRD write failed.");
+  }
+
+  if (!SwitchToAutoSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+    return 2;
+  }
+  return 0;
+}
+
+units::degree_t ADIS16448_IMU::GetAngle() const {
+  switch (m_yaw_axis) {
+    case kX:
+      return GetGyroAngleX();
+    case kY:
+      return GetGyroAngleY();
+    case kZ:
+      return GetGyroAngleZ();
+    default:
+      return 0_deg;
+  }
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetRate() const {
+  switch (m_yaw_axis) {
+    case kX:
+      return GetGyroRateX();
+    case kY:
+      return GetGyroRateY();
+    case kZ:
+      return GetGyroRateZ();
+    default:
+      return 0_deg_per_s;
+  }
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleX() const {
+  if (m_simGyroAngleX) {
+    return units::degree_t{m_simGyroAngleX.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_integ_gyro_angle_x};
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleY() const {
+  if (m_simGyroAngleY) {
+    return units::degree_t{m_simGyroAngleY.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_integ_gyro_angle_y};
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleZ() const {
+  if (m_simGyroAngleZ) {
+    return units::degree_t{m_simGyroAngleZ.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_integ_gyro_angle_z};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateX() const {
+  if (m_simGyroRateX) {
+    return units::degrees_per_second_t{m_simGyroRateX.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degrees_per_second_t{m_gyro_rate_x};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateY() const {
+  if (m_simGyroRateY) {
+    return units::degrees_per_second_t{m_simGyroRateY.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degrees_per_second_t{m_gyro_rate_y};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateZ() const {
+  if (m_simGyroRateZ) {
+    return units::degrees_per_second_t{m_simGyroRateZ.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degrees_per_second_t{m_gyro_rate_z};
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelX() const {
+  if (m_simAccelX) {
+    return units::meters_per_second_squared_t{m_simAccelX.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return m_accel_x * 9.81_mps_sq;
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelY() const {
+  if (m_simAccelY) {
+    return units::meters_per_second_squared_t{m_simAccelY.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return m_accel_y * 9.81_mps_sq;
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelZ() const {
+  if (m_simAccelZ) {
+    return units::meters_per_second_squared_t{m_simAccelZ.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return m_accel_z * 9.81_mps_sq;
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldX() const {
+  std::scoped_lock sync(m_mutex);
+  return units::gauss_t{m_mag_x * 1e-3};
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldY() const {
+  std::scoped_lock sync(m_mutex);
+  return units::gauss_t{m_mag_y * 1e-3};
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldZ() const {
+  std::scoped_lock sync(m_mutex);
+  return units::gauss_t{m_mag_z * 1e-3};
+}
+
+units::degree_t ADIS16448_IMU::GetXComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleX};
+}
+
+units::degree_t ADIS16448_IMU::GetYComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleY};
+}
+
+units::degree_t ADIS16448_IMU::GetXFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleX};
+}
+
+units::degree_t ADIS16448_IMU::GetYFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleY};
+}
+
+units::pounds_per_square_inch_t ADIS16448_IMU::GetBarometricPressure() const {
+  std::scoped_lock sync(m_mutex);
+  return units::mbar_t{m_baro};
+}
+
+units::celsius_t ADIS16448_IMU::GetTemperature() const {
+  std::scoped_lock sync(m_mutex);
+  return units::celsius_t{m_temp};
+}
+
+ADIS16448_IMU::IMUAxis ADIS16448_IMU::GetYawAxis() const {
+  return m_yaw_axis;
+}
+
+int ADIS16448_IMU::SetYawAxis(IMUAxis yaw_axis) {
+  if (m_yaw_axis == yaw_axis) {
+    return 1;
+  } else {
+    m_yaw_axis = yaw_axis;
+    Reset();
+    return 0;
+  }
+}
+
+int ADIS16448_IMU::GetPort() const {
+  return m_spi_port;
+}
+
+/**
+ * @brief Builds a Sendable object to push IMU data to the driver station.
+ *
+ * This function pushes the most recent angle estimates for all axes to the
+ *driver station.
+ **/
+void ADIS16448_IMU::InitSendable(nt::NTSendableBuilder& builder) {
+  builder.SetSmartDashboardType("ADIS16448 IMU");
+  auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
+  builder.SetUpdateTable([=]() {
+    nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
+  });
+}
diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
new file mode 100644
index 0000000..f48d827
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
@@ -0,0 +1,809 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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-2020 Analog Devices Inc. All Rights Reserved.           */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*                                                                            */
+/* Juan Chong - frcsupport@analog.com                                         */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ADIS16470_IMU.h"
+
+#include <frc/DigitalInput.h>
+#include <frc/DigitalSource.h>
+#include <frc/DriverStation.h>
+#include <frc/Timer.h>
+
+#include <cmath>
+#include <string>
+
+#include <hal/HAL.h>
+#include <networktables/NTSendableBuilder.h>
+#include <wpi/numbers>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+
+/* Helpful conversion functions */
+static inline int32_t ToInt(const uint32_t* buf) {
+  return static_cast<int32_t>((buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) |
+                              buf[3]);
+}
+
+static inline int16_t BuffToShort(const uint32_t* buf) {
+  return (static_cast<int16_t>(buf[0]) << 8) | buf[1];
+}
+
+static inline uint16_t ToUShort(const uint8_t* buf) {
+  return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
+}
+
+using namespace frc;
+
+namespace {
+template <typename S, typename... Args>
+inline void ADISReportError(int32_t status, const char* file, int line,
+                            const char* function, const S& format,
+                            Args&&... args) {
+  frc::ReportErrorV(status, file, line, function, format,
+                    fmt::make_args_checked<Args...>(format, args...));
+}
+}  // namespace
+
+#define REPORT_WARNING(msg) \
+  ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
+#define REPORT_ERROR(msg) \
+  ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
+
+/**
+ * Constructor.
+ */
+ADIS16470_IMU::ADIS16470_IMU()
+    : ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {}
+
+ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
+                             CalibrationTime cal_time)
+    : m_yaw_axis(yaw_axis),
+      m_spi_port(port),
+      m_calibration_time(static_cast<uint16_t>(cal_time)),
+      m_simDevice("Gyro:ADIS16470", port) {
+  if (m_simDevice) {
+    m_simGyroAngleX =
+        m_simDevice.CreateDouble("gyro_angle_x", hal::SimDevice::kInput, 0.0);
+    m_simGyroAngleY =
+        m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0);
+    m_simGyroAngleZ =
+        m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0);
+    m_simGyroRateX =
+        m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
+    m_simGyroRateY =
+        m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
+    m_simGyroRateZ =
+        m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
+    m_simAccelX =
+        m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
+    m_simAccelY =
+        m_simDevice.CreateDouble("accel_y", hal::SimDevice::kInput, 0.0);
+    m_simAccelZ =
+        m_simDevice.CreateDouble("accel_z", hal::SimDevice::kInput, 0.0);
+  }
+
+  if (!m_simDevice) {
+    // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+    // Relies on the RIO hardware by default configuring an output as low
+    // and configuring an input as high Z. The 10k pull-up resistor internal to
+    // the IMU then forces the reset line high for normal operation.
+    DigitalOutput* m_reset_out =
+        new DigitalOutput(27);  // Drive SPI CS2 (IMU RST) low
+    Wait(10_ms);                // Wait 10ms
+    delete m_reset_out;
+    new DigitalInput(27);  // Set SPI CS2 (IMU RST) high
+    Wait(500_ms);          // Wait 500ms for reset to complete
+
+    // Configure standard SPI
+    if (!SwitchToStandardSPI()) {
+      return;
+    }
+
+    // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1)
+    // = 400Hz)
+    WriteRegister(DEC_RATE, 0x0004);
+    // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation
+    // and PoP
+    WriteRegister(MSC_CTRL, 0x0001);
+    // Configure IMU internal Bartlett filter
+    WriteRegister(FILT_CTRL, 0x0000);
+    // Configure continuous bias calibration time based on user setting
+    WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
+
+    // Notify DS that IMU calibration delay is active
+    REPORT_WARNING(
+        "ADIS16470 IMU Detected. Starting initial calibration delay.");
+
+    // Wait for samples to accumulate internal to the IMU (110% of user-defined
+    // time)
+    Wait(units::second_t{pow(2, m_calibration_time) / 2000 * 64 * 1.1});
+
+    // Write offset calibration command to IMU
+    WriteRegister(GLOB_CMD, 0x0001);
+
+    // Configure and enable auto SPI
+    if (!SwitchToAutoSPI()) {
+      return;
+    }
+
+    // Let the user know the IMU was initiallized successfully
+    REPORT_WARNING("ADIS16470 IMU Successfully Initialized!");
+
+    // Drive SPI CS3 (IMU ready LED) low (active low)
+    new DigitalOutput(28);
+  }
+
+  // Report usage and post data to DS
+  HAL_Report(HALUsageReporting::kResourceType_ADIS16470, 0);
+
+  wpi::SendableRegistry::AddLW(this, "ADIS16470", port);
+}
+
+/**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in standard SPI mode.
+ *
+ * This function switches the active SPI port to standard SPI and is used
+ *primarily when exiting auto SPI. Exiting auto SPI is required to read or write
+ *using SPI since the auto SPI configuration, once active, locks the SPI message
+ *being transacted. This function also verifies that the SPI port is operating
+ *in standard SPI mode by reading back the IMU product ID.
+ **/
+bool ADIS16470_IMU::SwitchToStandardSPI() {
+  // Check to see whether the acquire thread is active. If so, wait for it to
+  // stop producing data.
+  if (m_thread_active) {
+    m_thread_active = false;
+    while (!m_thread_idle) {
+      Wait(10_ms);
+    }
+    // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
+    if (m_spi != nullptr && m_auto_configured) {
+      m_spi->StopAuto();
+      // We need to get rid of all the garbage left in the auto SPI buffer after
+      // stopping it. Sometimes data magically reappears, so we have to check
+      // the buffer size a couple of times
+      //  to be sure we got it all. Yuck.
+      uint32_t trashBuffer[200];
+      Wait(100_ms);
+      int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+      while (data_count > 0) {
+        /* Receive data, max of 200 words at a time (prevent potential segfault)
+         */
+        m_spi->ReadAutoReceivedData(trashBuffer, std::min(data_count, 200),
+                                    0_s);
+        /*Get the reamining data count */
+        data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+      }
+    }
+  }
+  // There doesn't seem to be a SPI port active. Let's try to set one up
+  if (m_spi == nullptr) {
+    m_spi = new SPI(m_spi_port);
+    m_spi->SetClockRate(2000000);
+    m_spi->SetMSBFirst();
+    m_spi->SetSampleDataOnTrailingEdge();
+    m_spi->SetClockActiveLow();
+    m_spi->SetChipSelectActiveLow();
+    ReadRegister(PROD_ID);  // Dummy read
+
+    // Validate the product ID
+    uint16_t prod_id = ReadRegister(PROD_ID);
+    if (prod_id != 16982 && prod_id != 16470) {
+      REPORT_ERROR("Could not find ADIS16470!");
+      Close();
+      return false;
+    }
+    return true;
+  } else {
+    // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
+    // product ID.
+    ReadRegister(PROD_ID);  // Dummy read
+    uint16_t prod_id = ReadRegister(PROD_ID);
+    if (prod_id != 16982 && prod_id != 16470) {
+      REPORT_ERROR("Could not find ADIS16470!");
+      Close();
+      return false;
+    } else {
+      return true;
+    }
+  }
+}
+
+/**
+ * @brief Switches to auto SPI operation. Primarily used when exiting standard
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in auto SPI mode.
+ *
+ * This function switches the active SPI port to auto SPI and is used primarily
+ *when exiting standard SPI. Auto SPI is required to asynchronously read data
+ *over SPI as it utilizes special FPGA hardware to react to an external data
+ *ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
+ *can be read by the CPU asynchronously. Standard SPI transactions are
+ * impossible on the selected SPI port once auto SPI is enabled. The stall
+ *settings, GPIO interrupt pin, and data packet settings used in this function
+ *are hard-coded to work only with the ADIS16470 IMU.
+ **/
+bool ADIS16470_IMU::SwitchToAutoSPI() {
+  // No SPI port has been set up. Go set one up first.
+  if (m_spi == nullptr) {
+    if (!SwitchToStandardSPI()) {
+      REPORT_ERROR("Failed to start/restart auto SPI");
+      return false;
+    }
+  }
+  // Only set up the interrupt if needed.
+  if (m_auto_interrupt == nullptr) {
+    m_auto_interrupt = new DigitalInput(26);
+  }
+  // The auto SPI controller gets angry if you try to set up two instances on
+  // one bus.
+  if (!m_auto_configured) {
+    m_spi->InitAuto(8200);
+    m_auto_configured = true;
+  }
+  // Do we need to change auto SPI settings?
+  switch (m_yaw_axis) {
+    case kX:
+      m_spi->SetAutoTransmitData(m_autospi_x_packet, 2);
+      break;
+    case kY:
+      m_spi->SetAutoTransmitData(m_autospi_y_packet, 2);
+      break;
+    default:
+      m_spi->SetAutoTransmitData(m_autospi_z_packet, 2);
+      break;
+  }
+  // Configure auto stall time
+  m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
+  // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
+  // activated) DR High = Data good (data capture should be triggered on the
+  // rising edge)
+  m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
+  // Check to see if the acquire thread is running. If not, kick one off.
+  if (!m_thread_idle) {
+    m_first_run = true;
+    m_thread_active = true;
+    m_acquire_task = std::thread(&ADIS16470_IMU::Acquire, this);
+  } else {
+    m_first_run = true;
+    m_thread_active = true;
+  }
+  // Looks like the thread didn't start for some reason. Abort.
+  /*
+  if(!m_thread_idle) {
+    REPORT_ERROR("Failed to start/restart the acquire() thread.");
+    Close();
+    return false;
+  }
+  */
+  return true;
+}
+
+/**
+ * @brief Switches the active SPI port to standard SPI mode, writes a new value
+ *to the NULL_CNFG register in the IMU, and re-enables auto SPI.
+ *
+ * @param new_cal_time Calibration time to be set.
+ *
+ * @return An int indicating the success or failure of writing the new NULL_CNFG
+ *setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
+ *Failure
+ *
+ * This function enters standard SPI mode, writes a new NULL_CNFG setting to the
+ *IMU, and re-enters auto SPI mode. This function does not include a blocking
+ *sleep, so the user must keep track of the elapsed offset calibration time
+ * themselves. After waiting the configured calibration time, the Calibrate()
+ *function should be called to activate the new offset calibration.
+ **/
+int ADIS16470_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
+  if (m_calibration_time == static_cast<uint16_t>(new_cal_time)) {
+    return 1;
+  }
+  if (!SwitchToStandardSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+    return 2;
+  }
+  m_calibration_time = static_cast<uint16_t>(new_cal_time);
+  WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
+  if (!SwitchToAutoSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+    return 2;
+  }
+  return 0;
+}
+
+/**
+ * @brief Switches the active SPI port to standard SPI mode, writes a new value
+ *to the DECIMATE register in the IMU, and re-enables auto SPI.
+ *
+ * @param reg Decimation value to be set.
+ *
+ * @return An int indicating the success or failure of writing the new DECIMATE
+ *setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
+ *Failure
+ *
+ * This function enters standard SPI mode, writes a new DECIMATE setting to the
+ *IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
+ **/
+int ADIS16470_IMU::ConfigDecRate(uint16_t reg) {
+  uint16_t m_reg = reg;
+  if (!SwitchToStandardSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+    return 2;
+  }
+  if (m_reg > 1999) {
+    REPORT_ERROR("Attempted to write an invalid decimation value.");
+    m_reg = 1999;
+  }
+  m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
+  WriteRegister(DEC_RATE, m_reg);
+  if (!SwitchToAutoSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+    return 2;
+  }
+  return 0;
+}
+
+/**
+ * @brief Switches the active SPI port to standard SPI mode, writes the command
+ *to activate the new null configuration, and re-enables auto SPI.
+ *
+ * This function enters standard SPI mode, writes 0x0001 to the GLOB_CMD
+ *register (thus making the new offset active in the IMU), and re-enters auto
+ *SPI mode. This function does not include a blocking sleep, so the user must
+ *keep track of the elapsed offset calibration time themselves.
+ **/
+void ADIS16470_IMU::Calibrate() {
+  if (!SwitchToStandardSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+  }
+  WriteRegister(GLOB_CMD, 0x0001);
+  if (!SwitchToAutoSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+  }
+}
+
+/**
+ * @brief Reads the contents of a specified register location over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @return An unsigned, 16-bit number representing the contents of the requested
+ *register location.
+ *
+ * This function reads the contents of an 8-bit register location by
+ *transmitting the register location byte along with a null (0x00) byte using
+ *the standard WPILib API. The response (two bytes) is read back using the
+ *WPILib API and joined using a helper function. This function assumes the
+ *controller is set to standard SPI mode.
+ **/
+uint16_t ADIS16470_IMU::ReadRegister(uint8_t reg) {
+  uint8_t buf[2];
+  buf[0] = reg & 0x7f;
+  buf[1] = 0;
+
+  m_spi->Write(buf, 2);
+  m_spi->Read(false, buf, 2);
+
+  return ToUShort(buf);
+}
+
+/**
+ * @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
+ *locations over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @param val An unsigned, 16-bit value to be written to the specified register
+ *location.
+ *
+ * This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
+ *via SPI. The upper and lower bytes that make up the 16-bit value are split
+ *into two unsined, 8-bit values and written to the upper and lower addresses of
+ *the specified register value. Only the lower (base) address must be specified.
+ *This function assumes the controller is set to standard SPI mode.
+ **/
+void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) {
+  uint8_t buf[2];
+  buf[0] = 0x80 | reg;
+  buf[1] = val & 0xff;
+  m_spi->Write(buf, 2);
+  buf[0] = 0x81 | reg;
+  buf[1] = val >> 8;
+  m_spi->Write(buf, 2);
+}
+
+/**
+ * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
+ *
+ * This function resets (zeros) the accumulated (integrated) angle estimates for
+ *the xgyro, ygyro, and zgyro outputs.
+ **/
+void ADIS16470_IMU::Reset() {
+  std::scoped_lock sync(m_mutex);
+  m_integ_angle = 0.0;
+}
+
+void ADIS16470_IMU::Close() {
+  if (m_thread_active) {
+    m_thread_active = false;
+    if (m_acquire_task.joinable()) {
+      m_acquire_task.join();
+    }
+  }
+  if (m_spi != nullptr) {
+    if (m_auto_configured) {
+      m_spi->StopAuto();
+    }
+    delete m_spi;
+    m_auto_configured = false;
+    if (m_auto_interrupt != nullptr) {
+      delete m_auto_interrupt;
+      m_auto_interrupt = nullptr;
+    }
+    m_spi = nullptr;
+  }
+}
+
+ADIS16470_IMU::~ADIS16470_IMU() {
+  Close();
+}
+
+/**
+ * @brief Main acquisition loop. Typically called asynchronously and free-wheels
+ *while the robot code is active.
+ *
+ * This is the main acquisiton loop for the IMU. During each iteration, data
+ *read using auto SPI is extracted from the FPGA FIFO, split, scaled, and
+ *integrated. Each X, Y, and Z value is 32-bits split across 4 indices (bytes)
+ *in the buffer. Auto SPI puts one byte in each index location. Each index is
+ *32-bits wide because the timestamp is an unsigned 32-bit int. The timestamp is
+ *always located at the beginning of the frame. Two indices (request_1 and
+ *request_2 below) are always invalid (garbage) and can be disregarded.
+ *
+ * Data order: [timestamp, request_1, request_2, x_1, x_2, x_3, x_4, y_1, y_2,
+ *y_3, y_4, z_1, z_2, z_3, z_4] x = delta x (32-bit x_1 = highest bit) y = delta
+ *y (32-bit y_1 = highest bit) z = delta z (32-bit z_1 = highest bit)
+ *
+ * Complementary filter code was borrowed from
+ *https://github.com/tcleg/Six_Axis_Complementary_Filter
+ **/
+void ADIS16470_IMU::Acquire() {
+  // Set data packet length
+  const int dataset_len = 19;  // 18 data points + timestamp
+
+  /* Fixed buffer size */
+  const int BUFFER_SIZE = 4000;
+
+  // This buffer can contain many datasets
+  uint32_t buffer[BUFFER_SIZE];
+  int data_count = 0;
+  int data_remainder = 0;
+  int data_to_read = 0;
+  uint32_t previous_timestamp = 0;
+  double delta_angle = 0.0;
+  double gyro_rate_x = 0.0;
+  double gyro_rate_y = 0.0;
+  double gyro_rate_z = 0.0;
+  double accel_x = 0.0;
+  double accel_y = 0.0;
+  double accel_z = 0.0;
+  double gyro_rate_x_si = 0.0;
+  double gyro_rate_y_si = 0.0;
+  // double gyro_rate_z_si = 0.0;
+  double accel_x_si = 0.0;
+  double accel_y_si = 0.0;
+  double accel_z_si = 0.0;
+  double compAngleX = 0.0;
+  double compAngleY = 0.0;
+  double accelAngleX = 0.0;
+  double accelAngleY = 0.0;
+
+  while (true) {
+    // Sleep loop for 10ms (wait for data)
+    Wait(10_ms);
+
+    if (m_thread_active) {
+      m_thread_idle = false;
+
+      data_count = m_spi->ReadAutoReceivedData(
+          buffer, 0,
+          0_s);  // Read number of bytes currently stored in the buffer
+      data_remainder =
+          data_count % dataset_len;  // Check if frame is incomplete. Add 1
+                                     // because of timestamp
+      data_to_read = data_count -
+                     data_remainder;  // Remove incomplete data from read count
+      /* Want to cap the data to read in a single read at the buffer size */
+      if (data_to_read > BUFFER_SIZE) {
+        REPORT_WARNING(
+            "ADIS16470 data processing thread overrun has occurred!");
+        data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+      }
+      m_spi->ReadAutoReceivedData(
+          buffer, data_to_read,
+          0_s);  // Read data from DMA buffer (only complete sets)
+
+      // Could be multiple data sets in the buffer. Handle each one.
+      for (int i = 0; i < data_to_read; i += dataset_len) {
+        // Timestamp is at buffer[i]
+        m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
+        /* Get delta angle value for selected yaw axis and scale by the elapsed
+         * time (based on timestamp) */
+        delta_angle = (ToInt(&buffer[i + 3]) * delta_angle_sf) /
+                      (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+        gyro_rate_x = (BuffToShort(&buffer[i + 7]) / 10.0);
+        gyro_rate_y = (BuffToShort(&buffer[i + 9]) / 10.0);
+        gyro_rate_z = (BuffToShort(&buffer[i + 11]) / 10.0);
+        accel_x = (BuffToShort(&buffer[i + 13]) / 800.0);
+        accel_y = (BuffToShort(&buffer[i + 15]) / 800.0);
+        accel_z = (BuffToShort(&buffer[i + 17]) / 800.0);
+
+        // Convert scaled sensor data to SI units
+        gyro_rate_x_si = gyro_rate_x * deg_to_rad;
+        gyro_rate_y_si = gyro_rate_y * deg_to_rad;
+        // gyro_rate_z_si = gyro_rate_z * deg_to_rad;
+        accel_x_si = accel_x * grav;
+        accel_y_si = accel_y * grav;
+        accel_z_si = accel_z * grav;
+
+        // Store timestamp for next iteration
+        previous_timestamp = buffer[i];
+
+        m_alpha = m_tau / (m_tau + m_dt);
+
+        if (m_first_run) {
+          accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
+                                                 (accel_z_si * accel_z_si)));
+          accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
+                                                 (accel_z_si * accel_z_si)));
+          compAngleX = accelAngleX;
+          compAngleY = accelAngleY;
+        } else {
+          // Process X angle
+          accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
+                                                 (accel_z_si * accel_z_si)));
+          accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
+                                                 (accel_z_si * accel_z_si)));
+          accelAngleX = FormatAccelRange(accelAngleX, accel_z_si);
+          accelAngleY = FormatAccelRange(accelAngleY, accel_z_si);
+          compAngleX =
+              CompFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
+          compAngleY =
+              CompFilterProcess(compAngleY, accelAngleY, gyro_rate_x_si);
+        }
+
+        {
+          std::scoped_lock sync(m_mutex);
+          /* Push data to global variables */
+          if (m_first_run) {
+            /* Don't accumulate first run. previous_timestamp will be "very" old
+             * and the integration will end up way off */
+            m_integ_angle = 0.0;
+          } else {
+            m_integ_angle += delta_angle;
+          }
+          m_gyro_rate_x = gyro_rate_x;
+          m_gyro_rate_y = gyro_rate_y;
+          m_gyro_rate_z = gyro_rate_z;
+          m_accel_x = accel_x;
+          m_accel_y = accel_y;
+          m_accel_z = accel_z;
+          m_compAngleX = compAngleX * rad_to_deg;
+          m_compAngleY = compAngleY * rad_to_deg;
+          m_accelAngleX = accelAngleX * rad_to_deg;
+          m_accelAngleY = accelAngleY * rad_to_deg;
+        }
+        m_first_run = false;
+      }
+    } else {
+      m_thread_idle = true;
+      data_count = 0;
+      data_remainder = 0;
+      data_to_read = 0;
+      previous_timestamp = 0.0;
+      delta_angle = 0.0;
+      gyro_rate_x = 0.0;
+      gyro_rate_y = 0.0;
+      gyro_rate_z = 0.0;
+      accel_x = 0.0;
+      accel_y = 0.0;
+      accel_z = 0.0;
+      gyro_rate_x_si = 0.0;
+      gyro_rate_y_si = 0.0;
+      // gyro_rate_z_si = 0.0;
+      accel_x_si = 0.0;
+      accel_y_si = 0.0;
+      accel_z_si = 0.0;
+      compAngleX = 0.0;
+      compAngleY = 0.0;
+      accelAngleX = 0.0;
+      accelAngleY = 0.0;
+    }
+  }
+}
+
+/* Complementary filter functions */
+double ADIS16470_IMU::FormatFastConverge(double compAngle, double accAngle) {
+  if (compAngle > accAngle + wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  } else if (accAngle > compAngle + wpi::numbers::pi) {
+    compAngle = compAngle + 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+double ADIS16470_IMU::FormatRange0to2PI(double compAngle) {
+  while (compAngle >= 2 * wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  }
+  while (compAngle < 0.0) {
+    compAngle = compAngle + 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+double ADIS16470_IMU::FormatAccelRange(double accelAngle, double accelZ) {
+  if (accelZ < 0.0) {
+    accelAngle = wpi::numbers::pi - accelAngle;
+  } else if (accelZ > 0.0 && accelAngle < 0.0) {
+    accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
+  }
+  return accelAngle;
+}
+
+double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
+                                        double omega) {
+  compAngle = FormatFastConverge(compAngle, accelAngle);
+  compAngle =
+      m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+  compAngle = FormatRange0to2PI(compAngle);
+  if (compAngle > wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+units::degree_t ADIS16470_IMU::GetAngle() const {
+  switch (m_yaw_axis) {
+    case kX:
+      if (m_simGyroAngleX) {
+        return units::degree_t{m_simGyroAngleX.Get()};
+      }
+      break;
+    case kY:
+      if (m_simGyroAngleY) {
+        return units::degree_t{m_simGyroAngleY.Get()};
+      }
+      break;
+    case kZ:
+      if (m_simGyroAngleZ) {
+        return units::degree_t{m_simGyroAngleZ.Get()};
+      }
+      break;
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_integ_angle};
+}
+
+units::degrees_per_second_t ADIS16470_IMU::GetRate() const {
+  if (m_yaw_axis == kX) {
+    if (m_simGyroRateX) {
+      return units::degrees_per_second_t{m_simGyroRateX.Get()};
+    }
+    std::scoped_lock sync(m_mutex);
+    return units::degrees_per_second_t{m_gyro_rate_x};
+  } else if (m_yaw_axis == kY) {
+    if (m_simGyroRateY) {
+      return units::degrees_per_second_t{m_simGyroRateY.Get()};
+    }
+    std::scoped_lock sync(m_mutex);
+    return units::degrees_per_second_t{m_gyro_rate_y};
+  } else if (m_yaw_axis == kZ) {
+    if (m_simGyroRateZ) {
+      return units::degrees_per_second_t{m_simGyroRateZ.Get()};
+    }
+    std::scoped_lock sync(m_mutex);
+    return units::degrees_per_second_t{m_gyro_rate_z};
+  } else {
+    return 0_deg_per_s;
+  }
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const {
+  if (m_simAccelX) {
+    return units::meters_per_second_squared_t{m_simAccelX.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::meters_per_second_squared_t{m_accel_x};
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelY() const {
+  if (m_simAccelY) {
+    return units::meters_per_second_squared_t{m_simAccelY.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::meters_per_second_squared_t{m_accel_y};
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelZ() const {
+  if (m_simAccelZ) {
+    return units::meters_per_second_squared_t{m_simAccelZ.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::meters_per_second_squared_t{m_accel_z};
+}
+
+units::degree_t ADIS16470_IMU::GetXComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleX};
+}
+
+units::degree_t ADIS16470_IMU::GetYComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleY};
+}
+
+units::degree_t ADIS16470_IMU::GetXFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleX};
+}
+
+units::degree_t ADIS16470_IMU::GetYFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleY};
+}
+
+ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const {
+  return m_yaw_axis;
+}
+
+int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) {
+  if (m_yaw_axis == yaw_axis) {
+    return 1;
+  }
+  if (!SwitchToStandardSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+    return 2;
+  }
+  m_yaw_axis = yaw_axis;
+  if (!SwitchToAutoSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+    return 2;
+  }
+  return 0;
+}
+
+int ADIS16470_IMU::GetPort() const {
+  return m_spi_port;
+}
+
+/**
+ * @brief Builds a Sendable object to push IMU data to the driver station.
+ *
+ * This function pushes the most recent angle estimates for all axes to the
+ *driver station.
+ **/
+void ADIS16470_IMU::InitSendable(nt::NTSendableBuilder& builder) {
+  builder.SetSmartDashboardType("ADIS16470 IMU");
+  auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
+  builder.SetUpdateTable([=]() {
+    nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
+  });
+}
diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp
index 0302acb..d994b14 100644
--- a/wpilibc/src/main/native/cpp/Compressor.cpp
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -19,7 +19,7 @@
     throw FRC_MakeError(err::ResourceAlreadyAllocated, "{}", module);
   }
 
-  SetClosedLoopControl(true);
+  m_module->EnableCompressorDigital();
 
   HAL_Report(HALUsageReporting::kResourceType_Compressor, module + 1);
   wpi::SendableRegistry::AddLW(this, "Compressor", module);
@@ -33,11 +33,11 @@
 }
 
 void Compressor::Start() {
-  SetClosedLoopControl(true);
+  EnableDigital();
 }
 
 void Compressor::Stop() {
-  SetClosedLoopControl(false);
+  Disable();
 }
 
 bool Compressor::Enabled() const {
@@ -48,24 +48,43 @@
   return m_module->GetPressureSwitch();
 }
 
-double Compressor::GetCompressorCurrent() const {
+units::ampere_t Compressor::GetCurrent() const {
   return m_module->GetCompressorCurrent();
 }
 
-void Compressor::SetClosedLoopControl(bool on) {
-  m_module->SetClosedLoopControl(on);
+units::volt_t Compressor::GetAnalogVoltage() const {
+  return m_module->GetAnalogVoltage(0);
 }
 
-bool Compressor::GetClosedLoopControl() const {
-  return m_module->GetClosedLoopControl();
+units::pounds_per_square_inch_t Compressor::GetPressure() const {
+  return m_module->GetPressure(0);
+}
+
+void Compressor::Disable() {
+  m_module->DisableCompressor();
+}
+
+void Compressor::EnableDigital() {
+  m_module->EnableCompressorDigital();
+}
+
+void Compressor::EnableAnalog(units::pounds_per_square_inch_t minPressure,
+                              units::pounds_per_square_inch_t maxPressure) {
+  m_module->EnableCompressorAnalog(minPressure, maxPressure);
+}
+
+void Compressor::EnableHybrid(units::pounds_per_square_inch_t minPressure,
+                              units::pounds_per_square_inch_t maxPressure) {
+  m_module->EnableCompressorHybrid(minPressure, maxPressure);
+}
+
+CompressorConfigType Compressor::GetConfigType() const {
+  return m_module->GetCompressorConfigType();
 }
 
 void Compressor::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Compressor");
   builder.AddBooleanProperty(
-      "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/wpilibc/src/main/native/cpp/Debouncer.cpp b/wpilibc/src/main/native/cpp/Debouncer.cpp
deleted file mode 100644
index eb402cd..0000000
--- a/wpilibc/src/main/native/cpp/Debouncer.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you 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/wpilibc/src/main/native/cpp/DigitalInput.cpp b/wpilibc/src/main/native/cpp/DigitalInput.cpp
index 7bce921..86f1c3a 100644
--- a/wpilibc/src/main/native/cpp/DigitalInput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -4,7 +4,6 @@
 
 #include "frc/DigitalInput.h"
 
-#include <iostream>
 #include <limits>
 
 #include <hal/DIO.h>
diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
index 8e994e9..452f5cd 100644
--- a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
+++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -89,6 +89,14 @@
     auto counter2 = m_counter->Get();
     auto pos2 = m_dutyCycle->GetOutput();
     if (counter == counter2 && pos == pos2) {
+      // map sensor range
+      if (pos < m_sensorMin) {
+        pos = m_sensorMin;
+      }
+      if (pos > m_sensorMax) {
+        pos = m_sensorMax;
+      }
+      pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
       units::turn_t turns{counter + pos - m_positionOffset};
       m_lastPosition = turns;
       return turns;
@@ -102,6 +110,11 @@
   return m_lastPosition;
 }
 
+void DutyCycleEncoder::SetDutyCycleRange(double min, double max) {
+  m_sensorMin = std::clamp(min, 0.0, 1.0);
+  m_sensorMax = std::clamp(max, 0.0, 1.0);
+}
+
 void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) {
   m_distancePerRotation = distancePerRotation;
   m_simDistancePerRotation.Set(distancePerRotation);
diff --git a/wpilibc/src/main/native/cpp/I2C.cpp b/wpilibc/src/main/native/cpp/I2C.cpp
index adf1c54..dcfb2b8 100644
--- a/wpilibc/src/main/native/cpp/I2C.cpp
+++ b/wpilibc/src/main/native/cpp/I2C.cpp
@@ -16,6 +16,14 @@
 I2C::I2C(Port port, int deviceAddress)
     : m_port(static_cast<HAL_I2CPort>(port)), m_deviceAddress(deviceAddress) {
   int32_t status = 0;
+
+  if (port == I2C::Port::kOnboard) {
+    FRC_ReportError(warn::Warning, "{}",
+                    "Onboard I2C port is subject to system lockups. See Known "
+                    "Issues page for "
+                    "details");
+  }
+
   HAL_InitializeI2C(m_port, &status);
   FRC_CheckErrorStatus(status, "Port {}", port);
 
diff --git a/wpilibc/src/main/native/cpp/MotorSafety.cpp b/wpilibc/src/main/native/cpp/MotorSafety.cpp
index 9cb28cd..388a887 100644
--- a/wpilibc/src/main/native/cpp/MotorSafety.cpp
+++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp
@@ -17,6 +17,15 @@
 static wpi::SmallPtrSet<MotorSafety*, 32> instanceList;
 static wpi::mutex listMutex;
 
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetMotorSafety() {
+  std::scoped_lock lock(listMutex);
+  instanceList.clear();
+}
+}  // namespace frc::impl
+#endif
+
 MotorSafety::MotorSafety() {
   std::scoped_lock lock(listMutex);
   instanceList.insert(this);
@@ -89,7 +98,15 @@
   if (stopTime < Timer::GetFPGATimestamp()) {
     FRC_ReportError(err::Timeout, "{}... Output not updated often enough",
                     GetDescription());
-    StopMotor();
+
+    try {
+      StopMotor();
+    } catch (frc::RuntimeError& e) {
+      e.Report();
+    } catch (std::exception& e) {
+      FRC_ReportError(err::Error, "{} StopMotor threw unexpected exception: {}",
+                      GetDescription(), e.what());
+    }
   }
 }
 
diff --git a/wpilibc/src/main/native/cpp/PneumaticHub.cpp b/wpilibc/src/main/native/cpp/PneumaticHub.cpp
index 8217e86..0f32e1e 100644
--- a/wpilibc/src/main/native/cpp/PneumaticHub.cpp
+++ b/wpilibc/src/main/native/cpp/PneumaticHub.cpp
@@ -4,6 +4,7 @@
 
 #include "frc/PneumaticHub.h"
 
+#include <fmt/format.h>
 #include <hal/REVPH.h>
 #include <wpi/NullDeleter.h>
 #include <wpi/StackTrace.h>
@@ -11,11 +12,27 @@
 #include "frc/Compressor.h"
 #include "frc/DoubleSolenoid.h"
 #include "frc/Errors.h"
+#include "frc/RobotBase.h"
 #include "frc/SensorUtil.h"
 #include "frc/Solenoid.h"
+#include "frc/fmt/Units.h"
 
 using namespace frc;
 
+/** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
+units::pounds_per_square_inch_t VoltsToPSI(units::volt_t sensorVoltage,
+                                           units::volt_t supplyVoltage) {
+  auto pressure = 250 * (sensorVoltage.value() / supplyVoltage.value()) - 25;
+  return units::pounds_per_square_inch_t{pressure};
+}
+
+/** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
+units::volt_t PSIToVolts(units::pounds_per_square_inch_t pressure,
+                         units::volt_t supplyVoltage) {
+  auto voltage = supplyVoltage.value() * (0.004 * pressure.value() + 0.1);
+  return units::volt_t{voltage};
+}
+
 wpi::mutex PneumaticHub::m_handleLock;
 std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<PneumaticHub::DataStore>>>
     PneumaticHub::m_handleMap = nullptr;
@@ -39,6 +56,38 @@
     m_moduleObject = PneumaticHub{handle, module};
     m_moduleObject.m_dataStore =
         std::shared_ptr<DataStore>{this, wpi::NullDeleter<DataStore>()};
+
+    auto version = m_moduleObject.GetVersion();
+
+    if (version.FirmwareMajor > 0 && RobotBase::IsReal()) {
+      // Write PH firmware version to roboRIO
+      std::FILE* file = nullptr;
+      file = std::fopen(
+          fmt::format("/tmp/frc_versions/REV_PH_{:0>2}_WPILib_Version.ini",
+                      module)
+              .c_str(),
+          "w");
+      if (file != nullptr) {
+        std::fputs("[Version]\n", file);
+        std::fputs(fmt::format("model=REV PH\n").c_str(), file);
+        std::fputs(fmt::format("deviceID={:x}\n", (0x9052600 | module)).c_str(),
+                   file);
+        std::fputs(fmt::format("currentVersion={}.{}.{}", version.FirmwareMajor,
+                               version.FirmwareMinor, version.FirmwareFix)
+                       .c_str(),
+                   file);
+        std::fclose(file);
+      }
+    }
+
+    // Check PH firmware version
+    if (version.FirmwareMajor > 0 && version.FirmwareMajor < 22) {
+      throw FRC_MakeError(
+          err::AssertionFailure,
+          "The Pneumatic Hub has firmware version {}.{}.{}, and must be "
+          "updated to version 2022.0.0 or later using the REV Hardware Client",
+          version.FirmwareMajor, version.FirmwareMinor, version.FirmwareFix);
+    }
   }
 
   ~DataStore() noexcept { HAL_FreeREVPH(m_moduleObject.m_handle); }
@@ -52,6 +101,7 @@
   bool m_compressorReserved{false};
   wpi::mutex m_reservedLock;
   PneumaticHub m_moduleObject{HAL_kInvalidHandle, 0};
+  std::array<units::millisecond_t, 16> m_oneShotDurMs{0_ms};
 };
 
 PneumaticHub::PneumaticHub()
@@ -76,47 +126,103 @@
 bool PneumaticHub::GetCompressor() const {
   int32_t status = 0;
   auto result = HAL_GetREVPHCompressor(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
-void PneumaticHub::SetClosedLoopControl(bool enabled) {
+void PneumaticHub::DisableCompressor() {
   int32_t status = 0;
-  HAL_SetREVPHClosedLoopControl(m_handle, enabled, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  HAL_SetREVPHClosedLoopControlDisabled(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
-bool PneumaticHub::GetClosedLoopControl() const {
+void PneumaticHub::EnableCompressorDigital() {
   int32_t status = 0;
-  auto result = HAL_GetREVPHClosedLoopControl(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
-  return result;
+  HAL_SetREVPHClosedLoopControlDigital(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticHub::EnableCompressorAnalog(
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
+  if (minPressure >= maxPressure) {
+    throw FRC_MakeError(err::InvalidParameter, "{}",
+                        "maxPressure must be greater than minPresure");
+  }
+  if (minPressure < 0_psi || minPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "minPressure must be between 0 and 120 PSI, got {}",
+                        minPressure);
+  }
+  if (maxPressure < 0_psi || maxPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "maxPressure must be between 0 and 120 PSI, got {}",
+                        maxPressure);
+  }
+  int32_t status = 0;
+  units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
+  units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
+  HAL_SetREVPHClosedLoopControlAnalog(m_handle, minAnalogVoltage.value(),
+                                      maxAnalogVoltage.value(), &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticHub::EnableCompressorHybrid(
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
+  if (minPressure >= maxPressure) {
+    throw FRC_MakeError(err::InvalidParameter, "{}",
+                        "maxPressure must be greater than minPresure");
+  }
+  if (minPressure < 0_psi || minPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "minPressure must be between 0 and 120 PSI, got {}",
+                        minPressure);
+  }
+  if (maxPressure < 0_psi || maxPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "maxPressure must be between 0 and 120 PSI, got {}",
+                        maxPressure);
+  }
+  int32_t status = 0;
+  units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
+  units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
+  HAL_SetREVPHClosedLoopControlHybrid(m_handle, minAnalogVoltage.value(),
+                                      maxAnalogVoltage.value(), &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+CompressorConfigType PneumaticHub::GetCompressorConfigType() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHCompressorConfig(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return static_cast<CompressorConfigType>(result);
 }
 
 bool PneumaticHub::GetPressureSwitch() const {
   int32_t status = 0;
   auto result = HAL_GetREVPHPressureSwitch(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
-double PneumaticHub::GetCompressorCurrent() const {
+units::ampere_t PneumaticHub::GetCompressorCurrent() const {
   int32_t status = 0;
   auto result = HAL_GetREVPHCompressorCurrent(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
-  return result;
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::ampere_t{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);
+  FRC_ReportError(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);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
@@ -125,27 +231,26 @@
 }
 
 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;
+  int32_t status = 0;
+  HAL_REVPHStickyFaults faults;
+  std::memset(&faults, 0, sizeof(faults));
+  HAL_GetREVPHStickyFaults(m_handle, &faults, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  uint32_t intFaults = 0;
+  static_assert(sizeof(faults) == sizeof(intFaults));
+  std::memcpy(&intFaults, &faults, sizeof(faults));
+  return intFaults & 0xFFFF;
 }
 
 void PneumaticHub::FireOneShot(int index) {
-  // TODO Fix me
-  // int32_t status = 0;
-  // HAL_FireREVPHOneShot(m_handle, index, &status);
-  // FRC_CheckErrorStatus(status, "Module {}", m_module);
+  int32_t status = 0;
+  HAL_FireREVPHOneShot(m_handle, index,
+                       m_dataStore->m_oneShotDurMs[index].value(), &status);
+  FRC_ReportError(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);
+  m_dataStore->m_oneShotDurMs[index] = duration;
 }
 
 bool PneumaticHub::CheckSolenoidChannel(int channel) const {
@@ -181,6 +286,98 @@
   m_dataStore->m_compressorReserved = false;
 }
 
+PneumaticHub::Version PneumaticHub::GetVersion() const {
+  int32_t status = 0;
+  HAL_REVPHVersion halVersions;
+  std::memset(&halVersions, 0, sizeof(halVersions));
+  HAL_GetREVPHVersion(m_handle, &halVersions, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PneumaticHub::Version versions;
+  static_assert(sizeof(halVersions) == sizeof(versions));
+  static_assert(std::is_standard_layout_v<decltype(versions)>);
+  static_assert(std::is_trivial_v<decltype(versions)>);
+  std::memcpy(&versions, &halVersions, sizeof(versions));
+  return versions;
+}
+
+PneumaticHub::Faults PneumaticHub::GetFaults() const {
+  int32_t status = 0;
+  HAL_REVPHFaults halFaults;
+  std::memset(&halFaults, 0, sizeof(halFaults));
+  HAL_GetREVPHFaults(m_handle, &halFaults, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PneumaticHub::Faults faults;
+  static_assert(sizeof(halFaults) == sizeof(faults));
+  static_assert(std::is_standard_layout_v<decltype(faults)>);
+  static_assert(std::is_trivial_v<decltype(faults)>);
+  std::memcpy(&faults, &halFaults, sizeof(faults));
+  return faults;
+}
+
+PneumaticHub::StickyFaults PneumaticHub::GetStickyFaults() const {
+  int32_t status = 0;
+  HAL_REVPHStickyFaults halStickyFaults;
+  std::memset(&halStickyFaults, 0, sizeof(halStickyFaults));
+  HAL_GetREVPHStickyFaults(m_handle, &halStickyFaults, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PneumaticHub::StickyFaults stickyFaults;
+  static_assert(sizeof(halStickyFaults) == sizeof(stickyFaults));
+  static_assert(std::is_standard_layout_v<decltype(stickyFaults)>);
+  static_assert(std::is_trivial_v<decltype(stickyFaults)>);
+  std::memcpy(&stickyFaults, &halStickyFaults, sizeof(stickyFaults));
+  return stickyFaults;
+}
+
+void PneumaticHub::ClearStickyFaults() {
+  int32_t status = 0;
+  HAL_ClearREVPHStickyFaults(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+units::volt_t PneumaticHub::GetInputVoltage() const {
+  int32_t status = 0;
+  auto voltage = HAL_GetREVPHVoltage(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::volt_t{voltage};
+}
+
+units::volt_t PneumaticHub::Get5VRegulatedVoltage() const {
+  int32_t status = 0;
+  auto voltage = HAL_GetREVPH5VVoltage(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::volt_t{voltage};
+}
+
+units::ampere_t PneumaticHub::GetSolenoidsTotalCurrent() const {
+  int32_t status = 0;
+  auto current = HAL_GetREVPHSolenoidCurrent(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::ampere_t{current};
+}
+
+units::volt_t PneumaticHub::GetSolenoidsVoltage() const {
+  int32_t status = 0;
+  auto voltage = HAL_GetREVPHSolenoidVoltage(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::volt_t{voltage};
+}
+
+units::volt_t PneumaticHub::GetAnalogVoltage(int channel) const {
+  int32_t status = 0;
+  auto voltage = HAL_GetREVPHAnalogVoltage(m_handle, channel, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::volt_t{voltage};
+}
+
+units::pounds_per_square_inch_t PneumaticHub::GetPressure(int channel) const {
+  int32_t status = 0;
+  auto sensorVoltage = HAL_GetREVPHAnalogVoltage(m_handle, channel, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  auto supplyVoltage = HAL_GetREVPH5VVoltage(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return VoltsToPSI(units::volt_t{sensorVoltage}, units::volt_t{supplyVoltage});
+}
+
 Solenoid PneumaticHub::MakeSolenoid(int channel) {
   return Solenoid{m_module, PneumaticsModuleType::REVPH, channel};
 }
diff --git a/wpilibc/src/main/native/cpp/PneumaticsBase.cpp b/wpilibc/src/main/native/cpp/PneumaticsBase.cpp
index 9d96651..bd9e033 100644
--- a/wpilibc/src/main/native/cpp/PneumaticsBase.cpp
+++ b/wpilibc/src/main/native/cpp/PneumaticsBase.cpp
@@ -4,6 +4,8 @@
 
 #include "frc/PneumaticsBase.h"
 
+#include <hal/REVPH.h>
+
 #include "frc/Errors.h"
 #include "frc/PneumaticHub.h"
 #include "frc/PneumaticsControlModule.h"
@@ -11,6 +13,19 @@
 
 using namespace frc;
 
+static_assert(
+    static_cast<int>(CompressorConfigType::Disabled) ==
+    HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kDisabled);
+static_assert(
+    static_cast<int>(CompressorConfigType::Digital) ==
+    HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kDigital);
+static_assert(
+    static_cast<int>(CompressorConfigType::Analog) ==
+    HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kAnalog);
+static_assert(
+    static_cast<int>(CompressorConfigType::Hybrid) ==
+    HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kHybrid);
+
 std::shared_ptr<PneumaticsBase> PneumaticsBase::GetForType(
     int module, PneumaticsModuleType moduleType) {
   if (moduleType == PneumaticsModuleType::CTREPCM) {
@@ -18,7 +33,8 @@
   } else if (moduleType == PneumaticsModuleType::REVPH) {
     return PneumaticHub::GetForModule(module);
   }
-  throw FRC_MakeError(err::InvalidParameter, "{}", moduleType);
+  throw FRC_MakeError(err::InvalidParameter, "{}",
+                      static_cast<int>(moduleType));
 }
 
 int PneumaticsBase::GetDefaultForType(PneumaticsModuleType moduleType) {
@@ -27,5 +43,6 @@
   } else if (moduleType == PneumaticsModuleType::REVPH) {
     return SensorUtil::GetDefaultREVPHModule();
   }
-  throw FRC_MakeError(err::InvalidParameter, "{}", moduleType);
+  throw FRC_MakeError(err::InvalidParameter, "{}",
+                      static_cast<int>(moduleType));
 }
diff --git a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
index 32de5ca..60cd84d 100644
--- a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
+++ b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
@@ -80,105 +80,128 @@
 bool PneumaticsControlModule::GetCompressor() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMCompressor(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
-void PneumaticsControlModule::SetClosedLoopControl(bool enabled) {
+void PneumaticsControlModule::DisableCompressor() {
   int32_t status = 0;
-  HAL_SetCTREPCMClosedLoopControl(m_handle, enabled, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  HAL_SetCTREPCMClosedLoopControl(m_handle, false, &status);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
-bool PneumaticsControlModule::GetClosedLoopControl() const {
+void PneumaticsControlModule::EnableCompressorDigital() {
+  int32_t status = 0;
+  HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::EnableCompressorAnalog(
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
+  int32_t status = 0;
+  HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::EnableCompressorHybrid(
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
+  int32_t status = 0;
+  HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+CompressorConfigType PneumaticsControlModule::GetCompressorConfigType() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMClosedLoopControl(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
-  return result;
+  FRC_ReportError(status, "Module {}", m_module);
+  return result ? CompressorConfigType::Digital
+                : CompressorConfigType::Disabled;
 }
 
 bool PneumaticsControlModule::GetPressureSwitch() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMPressureSwitch(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
-double PneumaticsControlModule::GetCompressorCurrent() const {
+units::ampere_t PneumaticsControlModule::GetCompressorCurrent() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMCompressorCurrent(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
-  return result;
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::ampere_t{result};
 }
 
 bool PneumaticsControlModule::GetCompressorCurrentTooHighFault() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMCompressorCurrentTooHighFault(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(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);
+  FRC_ReportError(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);
+  FRC_ReportError(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);
+  FRC_ReportError(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);
+  FRC_ReportError(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);
+  FRC_ReportError(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);
+  FRC_ReportError(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);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
 void PneumaticsControlModule::ClearAllStickyFaults() {
   int32_t status = 0;
   HAL_ClearAllCTREPCMStickyFaults(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(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);
+  FRC_ReportError(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);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
@@ -189,14 +212,14 @@
 int PneumaticsControlModule::GetSolenoidDisabledList() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMSolenoidDisabledList(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(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);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
 void PneumaticsControlModule::SetOneShotDuration(int index,
@@ -204,7 +227,7 @@
   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);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
 bool PneumaticsControlModule::CheckSolenoidChannel(int channel) const {
@@ -240,6 +263,15 @@
   m_dataStore->m_compressorReserved = false;
 }
 
+units::volt_t PneumaticsControlModule::GetAnalogVoltage(int channel) const {
+  return units::volt_t{0};
+}
+
+units::pounds_per_square_inch_t PneumaticsControlModule::GetPressure(
+    int channel) const {
+  return 0_psi;
+}
+
 Solenoid PneumaticsControlModule::MakeSolenoid(int channel) {
   return Solenoid{m_module, PneumaticsModuleType::CTREPCM, channel};
 }
diff --git a/wpilibc/src/main/native/cpp/PowerDistribution.cpp b/wpilibc/src/main/native/cpp/PowerDistribution.cpp
index 4c2acfc..21101f2 100644
--- a/wpilibc/src/main/native/cpp/PowerDistribution.cpp
+++ b/wpilibc/src/main/native/cpp/PowerDistribution.cpp
@@ -16,9 +16,6 @@
 #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>(
@@ -29,8 +26,21 @@
 
 using namespace frc;
 
-PowerDistribution::PowerDistribution()
-    : PowerDistribution(-1, ModuleType::kAutomatic) {}
+PowerDistribution::PowerDistribution() {
+  auto stack = wpi::GetStackTrace(1);
+
+  int32_t status = 0;
+  m_handle = HAL_InitializePowerDistribution(
+      kDefaultModule,
+      HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic,
+      stack.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Module {}", kDefaultModule);
+  m_module = HAL_GetPowerDistributionModuleNumber(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+
+  HAL_Report(HALUsageReporting::kResourceType_PDP, m_module + 1);
+  wpi::SendableRegistry::AddLW(this, "PowerDistribution", m_module);
+}
 
 PowerDistribution::PowerDistribution(int module, ModuleType moduleType) {
   auto stack = wpi::GetStackTrace(1);
@@ -114,6 +124,13 @@
   return m_module;
 }
 
+PowerDistribution::ModuleType PowerDistribution::GetType() const {
+  int32_t status = 0;
+  auto type = HAL_GetPowerDistributionType(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return static_cast<ModuleType>(type);
+}
+
 bool PowerDistribution::GetSwitchableChannel() const {
   int32_t status = 0;
   bool state = HAL_GetPowerDistributionSwitchableChannel(m_handle, &status);
@@ -127,20 +144,85 @@
   FRC_ReportError(status, "Module {}", m_module);
 }
 
+PowerDistribution::Version PowerDistribution::GetVersion() const {
+  int32_t status = 0;
+  HAL_PowerDistributionVersion halVersion;
+  std::memset(&halVersion, 0, sizeof(halVersion));
+  HAL_GetPowerDistributionVersion(m_handle, &halVersion, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PowerDistribution::Version version;
+  static_assert(sizeof(halVersion) == sizeof(version));
+  static_assert(std::is_standard_layout_v<decltype(version)>);
+  static_assert(std::is_trivial_v<decltype(version)>);
+  std::memcpy(&version, &halVersion, sizeof(version));
+  return version;
+}
+
+PowerDistribution::Faults PowerDistribution::GetFaults() const {
+  int32_t status = 0;
+  HAL_PowerDistributionFaults halFaults;
+  std::memset(&halFaults, 0, sizeof(halFaults));
+  HAL_GetPowerDistributionFaults(m_handle, &halFaults, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PowerDistribution::Faults faults;
+  static_assert(sizeof(halFaults) == sizeof(faults));
+  static_assert(std::is_standard_layout_v<decltype(faults)>);
+  static_assert(std::is_trivial_v<decltype(faults)>);
+  std::memcpy(&faults, &halFaults, sizeof(faults));
+  return faults;
+}
+
+PowerDistribution::StickyFaults PowerDistribution::GetStickyFaults() const {
+  int32_t status = 0;
+  HAL_PowerDistributionStickyFaults halStickyFaults;
+  std::memset(&halStickyFaults, 0, sizeof(halStickyFaults));
+  HAL_GetPowerDistributionStickyFaults(m_handle, &halStickyFaults, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PowerDistribution::StickyFaults stickyFaults;
+  static_assert(sizeof(halStickyFaults) == sizeof(stickyFaults));
+  static_assert(std::is_standard_layout_v<decltype(stickyFaults)>);
+  static_assert(std::is_trivial_v<decltype(stickyFaults)>);
+  std::memcpy(&stickyFaults, &halStickyFaults, sizeof(stickyFaults));
+  return stickyFaults;
+}
+
 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);
+  // Use manual reads to avoid printing errors
   for (int i = 0; i < numChannels; ++i) {
     builder.AddDoubleProperty(
-        fmt::format("Chan{}", i), [=] { return GetCurrent(i); }, nullptr);
+        fmt::format("Chan{}", i),
+        [=] {
+          int32_t lStatus = 0;
+          return HAL_GetPowerDistributionChannelCurrent(m_handle, i, &lStatus);
+        },
+        nullptr);
   }
   builder.AddDoubleProperty(
-      "Voltage", [=] { return GetVoltage(); }, nullptr);
+      "Voltage",
+      [=] {
+        int32_t lStatus = 0;
+        return HAL_GetPowerDistributionVoltage(m_handle, &lStatus);
+      },
+      nullptr);
   builder.AddDoubleProperty(
-      "TotalCurrent", [=] { return GetTotalCurrent(); }, nullptr);
+      "TotalCurrent",
+      [=] {
+        int32_t lStatus = 0;
+        return HAL_GetPowerDistributionTotalCurrent(m_handle, &lStatus);
+      },
+      nullptr);
   builder.AddBooleanProperty(
-      "SwitchableChannel", [=] { return GetSwitchableChannel(); },
-      [=](bool value) { SetSwitchableChannel(value); });
+      "SwitchableChannel",
+      [=] {
+        int32_t lStatus = 0;
+        return HAL_GetPowerDistributionSwitchableChannel(m_handle, &lStatus);
+      },
+      [=](bool value) {
+        int32_t lStatus = 0;
+        HAL_SetPowerDistributionSwitchableChannel(m_handle, value, &lStatus);
+      });
 }
diff --git a/wpilibc/src/main/native/cpp/Preferences.cpp b/wpilibc/src/main/native/cpp/Preferences.cpp
index 0c82f54..4663875 100644
--- a/wpilibc/src/main/native/cpp/Preferences.cpp
+++ b/wpilibc/src/main/native/cpp/Preferences.cpp
@@ -30,6 +30,14 @@
   return instance;
 }
 
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetPreferencesInstance() {
+  GetInstance() = Instance();
+}
+}  // namespace frc::impl
+#endif
+
 Preferences* Preferences::GetInstance() {
   ::GetInstance();
   static Preferences instance;
@@ -79,6 +87,7 @@
 void Preferences::InitString(std::string_view key, std::string_view value) {
   auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultString(value);
+  entry.SetPersistent();
 }
 
 void Preferences::SetInt(std::string_view key, int value) {
@@ -94,6 +103,7 @@
 void Preferences::InitInt(std::string_view key, int value) {
   auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultDouble(value);
+  entry.SetPersistent();
 }
 
 void Preferences::SetDouble(std::string_view key, double value) {
@@ -109,6 +119,7 @@
 void Preferences::InitDouble(std::string_view key, double value) {
   auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultDouble(value);
+  entry.SetPersistent();
 }
 
 void Preferences::SetFloat(std::string_view key, float value) {
@@ -124,6 +135,7 @@
 void Preferences::InitFloat(std::string_view key, float value) {
   auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultDouble(value);
+  entry.SetPersistent();
 }
 
 void Preferences::SetBoolean(std::string_view key, bool value) {
@@ -139,6 +151,7 @@
 void Preferences::InitBoolean(std::string_view key, bool value) {
   auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultBoolean(value);
+  entry.SetPersistent();
 }
 
 void Preferences::SetLong(std::string_view key, int64_t value) {
@@ -154,6 +167,7 @@
 void Preferences::InitLong(std::string_view key, int64_t value) {
   auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultDouble(value);
+  entry.SetPersistent();
 }
 
 bool Preferences::ContainsKey(std::string_view key) {
diff --git a/wpilibc/src/main/native/cpp/counter/ExternalDirectionCounter.cpp b/wpilibc/src/main/native/cpp/counter/ExternalDirectionCounter.cpp
new file mode 100644
index 0000000..8c7bb58
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/counter/ExternalDirectionCounter.cpp
@@ -0,0 +1,103 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you 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/ExternalDirectionCounter.h"
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+ExternalDirectionCounter::ExternalDirectionCounter(
+    DigitalSource& countSource, DigitalSource& directionSource)
+    : ExternalDirectionCounter(
+          {&countSource, wpi::NullDeleter<DigitalSource>()},
+          {&directionSource, wpi::NullDeleter<DigitalSource>()}) {}
+
+ExternalDirectionCounter::ExternalDirectionCounter(
+    std::shared_ptr<DigitalSource> countSource,
+    std::shared_ptr<DigitalSource> directionSource) {
+  if (countSource == nullptr) {
+    throw FRC_MakeError(err::NullParameter, "{}", "countSource");
+  }
+  if (directionSource == nullptr) {
+    throw FRC_MakeError(err::NullParameter, "{}", "directionSource");
+  }
+
+  m_countSource = countSource;
+  m_directionSource = directionSource;
+
+  int32_t status = 0;
+  m_handle = HAL_InitializeCounter(
+      HAL_Counter_Mode::HAL_Counter_kExternalDirection, &m_index, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+
+  HAL_SetCounterUpSource(m_handle, m_countSource->GetPortHandleForRouting(),
+                         static_cast<HAL_AnalogTriggerType>(
+                             m_countSource->GetAnalogTriggerTypeForRouting()),
+                         &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+  HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+
+  HAL_SetCounterDownSource(
+      m_handle, m_directionSource->GetPortHandleForRouting(),
+      static_cast<HAL_AnalogTriggerType>(
+          m_directionSource->GetAnalogTriggerTypeForRouting()),
+      &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+  HAL_SetCounterDownSourceEdge(m_handle, false, true, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+
+  Reset();
+
+  HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+  wpi::SendableRegistry::AddLW(this, "External Direction Counter", m_index);
+}
+
+ExternalDirectionCounter::~ExternalDirectionCounter() {
+  int32_t status = 0;
+  HAL_FreeCounter(m_handle, &status);
+}
+
+int ExternalDirectionCounter::GetCount() const {
+  int32_t status = 0;
+  int val = HAL_GetCounter(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+  return val;
+}
+
+void ExternalDirectionCounter::SetReverseDirection(bool reverseDirection) {
+  int32_t status = 0;
+  HAL_SetCounterReverseDirection(m_handle, reverseDirection, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::Reset() {
+  int32_t status = 0;
+  HAL_ResetCounter(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::SetEdgeConfiguration(
+    EdgeConfiguration configuration) {
+  int32_t status = 0;
+  bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+                configuration == EdgeConfiguration::kBoth;
+  bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+                 configuration == EdgeConfiguration::kBoth;
+  HAL_SetCounterUpSourceEdge(m_handle, rising, falling, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("External Direction Counter");
+  builder.AddDoubleProperty(
+      "Count", [&] { return GetCount(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/counter/Tachometer.cpp b/wpilibc/src/main/native/cpp/counter/Tachometer.cpp
new file mode 100644
index 0000000..a52bea5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/counter/Tachometer.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 "frc/counter/Tachometer.h"
+
+#include <frc/DigitalSource.h>
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+Tachometer::Tachometer(DigitalSource& source)
+    : Tachometer({&source, wpi::NullDeleter<DigitalSource>()}) {}
+Tachometer::Tachometer(std::shared_ptr<DigitalSource> source) {
+  if (source == nullptr) {
+    throw FRC_MakeError(err::NullParameter, "{}", "source");
+  }
+
+  m_source = source;
+
+  int32_t status = 0;
+  HAL_SetCounterUpSource(m_handle, m_source->GetPortHandleForRouting(),
+                         static_cast<HAL_AnalogTriggerType>(
+                             m_source->GetAnalogTriggerTypeForRouting()),
+                         &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+  HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+
+  HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+  wpi::SendableRegistry::AddLW(this, "Tachometer", m_index);
+}
+
+Tachometer::~Tachometer() {
+  int32_t status = 0;
+  HAL_FreeCounter(m_handle, &status);
+}
+
+units::hertz_t Tachometer::GetFrequency() const {
+  auto period = GetPeriod();
+  if (period.to<double>() == 0) {
+    return units::hertz_t{0.0};
+  }
+  return 1 / period;
+}
+
+units::second_t Tachometer::GetPeriod() const {
+  int32_t status = 0;
+  double period = HAL_GetCounterPeriod(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+  return units::second_t{period};
+}
+
+int Tachometer::GetEdgesPerRevolution() const {
+  return m_edgesPerRevolution;
+}
+void Tachometer::SetEdgesPerRevolution(int edges) {
+  m_edgesPerRevolution = edges;
+}
+
+units::turns_per_second_t Tachometer::GetRevolutionsPerSecond() const {
+  auto period = GetPeriod();
+  if (period.to<double>() == 0) {
+    return units::turns_per_second_t{0.0};
+  }
+  int edgesPerRevolution = GetEdgesPerRevolution();
+  if (edgesPerRevolution == 0) {
+    return units::turns_per_second_t{0.0};
+  }
+  auto rotationHz = ((1.0 / edgesPerRevolution) / period);
+  return units::turns_per_second_t{rotationHz.to<double>()};
+}
+
+units::revolutions_per_minute_t Tachometer::GetRevolutionsPerMinute() const {
+  return units::revolutions_per_minute_t{GetRevolutionsPerSecond()};
+}
+
+bool Tachometer::GetStopped() const {
+  int32_t status = 0;
+  bool stopped = HAL_GetCounterStopped(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+  return stopped;
+}
+
+int Tachometer::GetSamplesToAverage() const {
+  int32_t status = 0;
+  int32_t samplesToAverage = HAL_GetCounterSamplesToAverage(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+  return samplesToAverage;
+}
+
+void Tachometer::SetSamplesToAverage(int samples) {
+  int32_t status = 0;
+  HAL_SetCounterSamplesToAverage(m_handle, samples, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::SetMaxPeriod(units::second_t maxPeriod) {
+  int32_t status = 0;
+  HAL_SetCounterMaxPeriod(m_handle, maxPeriod.to<double>(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::SetUpdateWhenEmpty(bool updateWhenEmpty) {
+  int32_t status = 0;
+  HAL_SetCounterUpdateWhenEmpty(m_handle, updateWhenEmpty, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Tachometer");
+  builder.AddDoubleProperty(
+      "RPS", [&] { return GetRevolutionsPerSecond().to<double>(); }, nullptr);
+  builder.AddDoubleProperty(
+      "RPM", [&] { return GetRevolutionsPerMinute().to<double>(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/counter/UpDownCounter.cpp b/wpilibc/src/main/native/cpp/counter/UpDownCounter.cpp
new file mode 100644
index 0000000..62537ee
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/counter/UpDownCounter.cpp
@@ -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.
+
+#include "frc/counter/UpDownCounter.h"
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+UpDownCounter::UpDownCounter(DigitalSource& upSource, DigitalSource& downSource)
+    : UpDownCounter({&upSource, wpi::NullDeleter<DigitalSource>()},
+                    {&downSource, wpi::NullDeleter<DigitalSource>()}) {}
+
+UpDownCounter::UpDownCounter(std::shared_ptr<DigitalSource> upSource,
+                             std::shared_ptr<DigitalSource> downSource) {
+  m_upSource = upSource;
+  m_downSource = downSource;
+
+  int32_t status = 0;
+  m_handle = HAL_InitializeCounter(HAL_Counter_Mode::HAL_Counter_kTwoPulse,
+                                   &m_index, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+
+  if (m_upSource) {
+    HAL_SetCounterUpSource(m_handle, m_upSource->GetPortHandleForRouting(),
+                           static_cast<HAL_AnalogTriggerType>(
+                               m_upSource->GetAnalogTriggerTypeForRouting()),
+                           &status);
+    FRC_CheckErrorStatus(status, "{}", m_index);
+    HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+    FRC_CheckErrorStatus(status, "{}", m_index);
+  }
+
+  if (m_downSource) {
+    HAL_SetCounterDownSource(
+        m_handle, m_downSource->GetPortHandleForRouting(),
+        static_cast<HAL_AnalogTriggerType>(
+            m_downSource->GetAnalogTriggerTypeForRouting()),
+        &status);
+    FRC_CheckErrorStatus(status, "{}", m_index);
+    HAL_SetCounterDownSourceEdge(m_handle, true, false, &status);
+    FRC_CheckErrorStatus(status, "{}", m_index);
+  }
+
+  Reset();
+
+  HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+  wpi::SendableRegistry::AddLW(this, "UpDown Counter", m_index);
+}
+
+UpDownCounter::~UpDownCounter() {
+  int32_t status = 0;
+  HAL_FreeCounter(m_handle, &status);
+}
+
+int UpDownCounter::GetCount() const {
+  int32_t status = 0;
+  int val = HAL_GetCounter(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+  return val;
+}
+
+void UpDownCounter::SetReverseDirection(bool reverseDirection) {
+  int32_t status = 0;
+  HAL_SetCounterReverseDirection(m_handle, reverseDirection, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::Reset() {
+  int32_t status = 0;
+  HAL_ResetCounter(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::SetUpEdgeConfiguration(EdgeConfiguration configuration) {
+  int32_t status = 0;
+  bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+                configuration == EdgeConfiguration::kBoth;
+  bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+                 configuration == EdgeConfiguration::kBoth;
+  HAL_SetCounterUpSourceEdge(m_handle, rising, falling, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::SetDownEdgeConfiguration(EdgeConfiguration configuration) {
+  int32_t status = 0;
+  bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+                configuration == EdgeConfiguration::kBoth;
+  bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+                 configuration == EdgeConfiguration::kBoth;
+  HAL_SetCounterDownSourceEdge(m_handle, rising, falling, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("UpDown Counter");
+  builder.AddDoubleProperty(
+      "Count", [&] { return GetCount(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index b54b607..eed9495 100644
--- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -112,9 +112,10 @@
   double maxInput =
       std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
 
-  if (xSpeed >= 0.0) {
+  // Sign is used because `xSpeed >= 0.0` succeeds for -0.0
+  if (!std::signbit(xSpeed)) {
     // First quadrant, else second quadrant
-    if (zRotation >= 0.0) {
+    if (!std::signbit(zRotation)) {
       leftSpeed = maxInput;
       rightSpeed = xSpeed - zRotation;
     } else {
@@ -123,7 +124,7 @@
     }
   } else {
     // Third quadrant, else fourth quadrant
-    if (zRotation >= 0.0) {
+    if (!std::signbit(zRotation)) {
       leftSpeed = xSpeed + zRotation;
       rightSpeed = maxInput;
     } else {
diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
index c847678..268ae64 100644
--- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -102,7 +102,7 @@
   wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
   wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
 
-  Normalize(wheelSpeeds);
+  Desaturate(wheelSpeeds);
 
   return {wheelSpeeds[kLeft], wheelSpeeds[kRight], wheelSpeeds[kBack]};
 }
diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index b74c611..70e90d4 100644
--- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -103,7 +103,7 @@
   wheelSpeeds[kRearLeft] = input.x - input.y + zRotation;
   wheelSpeeds[kRearRight] = input.x + input.y - zRotation;
 
-  Normalize(wheelSpeeds);
+  Desaturate(wheelSpeeds);
 
   return {wheelSpeeds[kFrontLeft], wheelSpeeds[kFrontRight],
           wheelSpeeds[kRearLeft], wheelSpeeds[kRearRight]};
diff --git a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
index 2159958..97ea0ee 100644
--- a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
+++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
@@ -35,7 +35,7 @@
   return frc::ApplyDeadband(value, deadband);
 }
 
-void RobotDriveBase::Normalize(wpi::span<double> wheelSpeeds) {
+void RobotDriveBase::Desaturate(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/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
index f8be7de..ac1c8ad 100644
--- a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -48,11 +48,23 @@
 };
 }  // namespace
 
-static Instance& GetInstance() {
-  static Instance instance;
+static std::unique_ptr<Instance>& GetInstanceHolder() {
+  static std::unique_ptr<Instance> instance = std::make_unique<Instance>();
   return instance;
 }
 
+static Instance& GetInstance() {
+  return *GetInstanceHolder();
+}
+
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetLiveWindow() {
+  std::make_unique<Instance>().swap(GetInstanceHolder());
+}
+}  // namespace frc::impl
+#endif
+
 std::shared_ptr<Component> Instance::GetOrAdd(wpi::Sendable* sendable) {
   auto data = std::static_pointer_cast<Component>(
       wpi::SendableRegistry::GetData(sendable, dataHandle));
diff --git a/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp b/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
index 9829a75..f9ff4b8 100644
--- a/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
+++ b/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
@@ -42,6 +42,10 @@
   return m_pwm.GetChannel();
 }
 
+void PWMMotorController::EnableDeadbandElimination(bool eliminateDeadband) {
+  m_pwm.EnableDeadbandElimination(eliminateDeadband);
+}
+
 PWMMotorController::PWMMotorController(std::string_view name, int channel)
     : m_pwm(channel, false) {
   wpi::SendableRegistry::AddLW(this, name, channel);
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
index 1675c64..d5e076a 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
@@ -8,19 +8,19 @@
 #include <memory>
 #include <string>
 
-#include <wpi/DenseMap.h>
+#include <wpi/StringMap.h>
 #include <wpi/sendable/SendableBuilder.h>
 #include <wpi/sendable/SendableRegistry.h>
 
 namespace frc {
 namespace detail {
 std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
-    CS_Source source) {
-  static wpi::DenseMap<int, std::shared_ptr<SendableCameraWrapper>> wrappers;
-  return wrappers[static_cast<int>(source)];
+    std::string_view cameraName) {
+  static wpi::StringMap<std::shared_ptr<SendableCameraWrapper>> wrappers;
+  return wrappers[cameraName];
 }
 
-void AddToSendableRegistry(wpi::Sendable* sendable, std::string name) {
+void AddToSendableRegistry(wpi::Sendable* sendable, std::string_view name) {
   wpi::SendableRegistry::Add(sendable, name);
 }
 }  // namespace detail
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
index 81f6671..31e4b11 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
@@ -31,6 +31,7 @@
     "Differential Drivebase",
     "Mecanum Drivebase",
     "Camera Stream",
+    "Field",
 };
 
 const char* detail::GetStringForWidgetType(BuiltInWidgets type) {
diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
new file mode 100644
index 0000000..46c412a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
@@ -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.
+
+#include "frc/simulation/ADIS16448_IMUSim.h"
+
+#include <frc/ADIS16448_IMU.h>
+#include <frc/simulation/SimDeviceSim.h>
+
+using namespace frc::sim;
+
+ADIS16448_IMUSim::ADIS16448_IMUSim(const frc::ADIS16448_IMU& imu) {
+  frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16448", imu.GetPort()};
+  m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
+  m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
+  m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
+  m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
+  m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
+  m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
+  m_simAccelX = deviceSim.GetDouble("accel_x");
+  m_simAccelY = deviceSim.GetDouble("accel_y");
+  m_simAccelZ = deviceSim.GetDouble("accel_z");
+}
+
+void ADIS16448_IMUSim::SetGyroAngleX(units::degree_t angle) {
+  m_simGyroAngleX.Set(angle.value());
+}
+
+void ADIS16448_IMUSim::SetGyroAngleY(units::degree_t angle) {
+  m_simGyroAngleY.Set(angle.value());
+}
+
+void ADIS16448_IMUSim::SetGyroAngleZ(units::degree_t angle) {
+  m_simGyroAngleZ.Set(angle.value());
+}
+
+void ADIS16448_IMUSim::SetGyroRateX(units::degrees_per_second_t angularRate) {
+  m_simGyroRateX.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) {
+  m_simGyroRateY.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) {
+  m_simGyroRateZ.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
+  m_simAccelX.Set(accel.value());
+}
+
+void ADIS16448_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
+  m_simAccelY.Set(accel.value());
+}
+
+void ADIS16448_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
+  m_simAccelZ.Set(accel.value());
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
new file mode 100644
index 0000000..40c09c3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
@@ -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.
+
+#include "frc/simulation/ADIS16470_IMUSim.h"
+
+#include <frc/ADIS16470_IMU.h>
+#include <frc/simulation/SimDeviceSim.h>
+
+using namespace frc::sim;
+
+ADIS16470_IMUSim::ADIS16470_IMUSim(const frc::ADIS16470_IMU& imu) {
+  frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16470", imu.GetPort()};
+  m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
+  m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
+  m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
+  m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
+  m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
+  m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
+  m_simAccelX = deviceSim.GetDouble("accel_x");
+  m_simAccelY = deviceSim.GetDouble("accel_y");
+  m_simAccelZ = deviceSim.GetDouble("accel_z");
+}
+
+void ADIS16470_IMUSim::SetGyroAngleX(units::degree_t angle) {
+  m_simGyroAngleX.Set(angle.value());
+}
+
+void ADIS16470_IMUSim::SetGyroAngleY(units::degree_t angle) {
+  m_simGyroAngleY.Set(angle.value());
+}
+
+void ADIS16470_IMUSim::SetGyroAngleZ(units::degree_t angle) {
+  m_simGyroAngleZ.Set(angle.value());
+}
+
+void ADIS16470_IMUSim::SetGyroRateX(units::degrees_per_second_t angularRate) {
+  m_simGyroRateX.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) {
+  m_simGyroRateY.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) {
+  m_simGyroRateZ.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
+  m_simAccelX.Set(accel.value());
+}
+
+void ADIS16470_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
+  m_simAccelY.Set(accel.value());
+}
+
+void ADIS16470_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
+  m_simAccelZ.Set(accel.value());
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
new file mode 100644
index 0000000..7860e32
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.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 "frc/simulation/DCMotorSim.h"
+
+#include <wpi/MathExtras.h>
+
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DCMotorSim::DCMotorSim(const LinearSystem<2, 1, 2>& plant,
+                       const DCMotor& gearbox, double gearing,
+                       const std::array<double, 2>& measurementStdDevs)
+    : LinearSystemSim<2, 1, 2>(plant, measurementStdDevs),
+      m_gearbox(gearbox),
+      m_gearing(gearing) {}
+
+DCMotorSim::DCMotorSim(const DCMotor& gearbox, double gearing,
+                       units::kilogram_square_meter_t moi,
+                       const std::array<double, 2>& measurementStdDevs)
+    : DCMotorSim(LinearSystemId::DCMotorSystem(gearbox, moi, gearing), gearbox,
+                 gearing, measurementStdDevs) {}
+
+units::radian_t DCMotorSim::GetAngularPosition() const {
+  return units::radian_t{GetOutput(0)};
+}
+
+units::radians_per_second_t DCMotorSim::GetAngularVelocity() const {
+  return units::radians_per_second_t{GetOutput(1)};
+}
+
+units::ampere_t DCMotorSim::GetCurrentDraw() const {
+  // I = V / R - omega / (Kv * R)
+  // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+  // is spinning 10x faster than the output.
+  return m_gearbox.Current(GetAngularVelocity() * m_gearing,
+                           units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
+}
+
+void DCMotorSim::SetInputVoltage(units::volt_t voltage) {
+  SetInput(Eigen::Vector<double, 1>{voltage.value()});
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
index 0bbf9c3..78b9dc7 100644
--- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -43,8 +43,8 @@
 
 Eigen::Vector<double, 2> DifferentialDrivetrainSim::ClampInput(
     const Eigen::Vector<double, 2>& u) {
-  return frc::NormalizeInputVector<2>(u,
-                                      frc::RobotController::GetInputVoltage());
+  return frc::DesaturateInputVector<2>(u,
+                                       frc::RobotController::GetInputVoltage());
 }
 
 void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage,
diff --git a/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp b/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
index dd6dba8..ca7384a 100644
--- a/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
@@ -73,21 +73,23 @@
   HALSIM_SetREVPHCompressorOn(m_index, compressorOn);
 }
 
-std::unique_ptr<CallbackStore> REVPHSim::RegisterClosedLoopEnabledCallback(
+std::unique_ptr<CallbackStore> REVPHSim::RegisterCompressorConfigTypeCallback(
     NotifyCallback callback, bool initialNotify) {
   auto store = std::make_unique<CallbackStore>(
-      m_index, -1, callback, &HALSIM_CancelREVPHClosedLoopEnabledCallback);
-  store->SetUid(HALSIM_RegisterREVPHClosedLoopEnabledCallback(
+      m_index, -1, callback, &HALSIM_CancelREVPHCompressorConfigTypeCallback);
+  store->SetUid(HALSIM_RegisterREVPHCompressorConfigTypeCallback(
       m_index, &CallbackStoreThunk, store.get(), initialNotify));
   return store;
 }
 
-bool REVPHSim::GetClosedLoopEnabled() const {
-  return HALSIM_GetREVPHClosedLoopEnabled(m_index);
+int REVPHSim::GetCompressorConfigType() const {
+  return HALSIM_GetREVPHCompressorConfigType(m_index);
 }
 
-void REVPHSim::SetClosedLoopEnabled(bool closedLoopEnabled) {
-  HALSIM_SetREVPHClosedLoopEnabled(m_index, closedLoopEnabled);
+void REVPHSim::SetCompressorConfigType(int compressorConfigType) {
+  HALSIM_SetREVPHCompressorConfigType(
+      m_index,
+      static_cast<HAL_REVPHCompressorConfigType>(compressorConfigType));
 }
 
 std::unique_ptr<CallbackStore> REVPHSim::RegisterPressureSwitchCallback(
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
index 175db0d..b49bc99 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
@@ -21,7 +21,6 @@
 
 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");
@@ -50,7 +49,20 @@
   Flush();
 }
 
+Color8Bit MechanismLigament2d::GetColor() {
+  std::scoped_lock lock(m_mutex);
+  if (m_colorEntry) {
+    auto color = m_colorEntry.GetString("");
+    std::strncpy(m_color, color.c_str(), sizeof(m_color));
+    m_color[sizeof(m_color) - 1] = '\0';
+  }
+  unsigned int r = 0, g = 0, b = 0;
+  std::sscanf(m_color, "#%02X%02X%02X", &r, &g, &b);
+  return {static_cast<int>(r), static_cast<int>(g), static_cast<int>(b)};
+}
+
 double MechanismLigament2d::GetAngle() {
+  std::scoped_lock lock(m_mutex);
   if (m_angleEntry) {
     m_angle = m_angleEntry.GetDouble(0.0);
   }
@@ -58,12 +70,21 @@
 }
 
 double MechanismLigament2d::GetLength() {
+  std::scoped_lock lock(m_mutex);
   if (m_lengthEntry) {
     m_length = m_lengthEntry.GetDouble(0.0);
   }
   return m_length;
 }
 
+double MechanismLigament2d::GetLineWeight() {
+  std::scoped_lock lock(m_mutex);
+  if (m_weightEntry) {
+    m_weight = m_weightEntry.GetDouble(0.0);
+  }
+  return m_weight;
+}
+
 void MechanismLigament2d::SetLength(double length) {
   std::scoped_lock lock(m_mutex);
   m_length = length;
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp
index 07d15f0..637b24b 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp
@@ -20,14 +20,12 @@
 }
 
 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);
   }
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
index 25bb61f..5088394 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -19,6 +19,8 @@
 
 namespace {
 struct Instance {
+  Instance() { HAL_Report(HALUsageReporting::kResourceType_SmartDashboard, 0); }
+
   detail::ListenerExecutor listenerExecutor;
   std::shared_ptr<nt::NetworkTable> table =
       nt::NetworkTableInstance::GetDefault().GetTable("SmartDashboard");
@@ -27,12 +29,23 @@
 };
 }  // namespace
 
-static Instance& GetInstance() {
-  HAL_Report(HALUsageReporting::kResourceType_SmartDashboard, 0);
-  static Instance instance;
+static std::unique_ptr<Instance>& GetInstanceHolder() {
+  static std::unique_ptr<Instance> instance = std::make_unique<Instance>();
   return instance;
 }
 
+static Instance& GetInstance() {
+  return *GetInstanceHolder();
+}
+
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetSmartDashboardInstance() {
+  std::make_unique<Instance>().swap(GetInstanceHolder());
+}
+}  // namespace frc::impl
+#endif
+
 void SmartDashboard::init() {
   GetInstance();
 }
diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp
index 78c286c..3433a02 100644
--- a/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -251,6 +251,9 @@
     }
   }
 
+  // Call DriverStation::InDisabled() to kick off DS thread
+  DriverStation::InDisabled(true);
+
   // First and one-time initialization
   inst.GetTable("LiveWindow")
       ->GetSubTable(".status")
diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
new file mode 100644
index 0000000..0dbfa68
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
@@ -0,0 +1,428 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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-2020 Analog Devices Inc. All Rights Reserved.           */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*                                                                            */
+/* Modified by Juan Chong - frcsupport@analog.com                             */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+#include <atomic>
+#include <memory>
+#include <thread>
+
+#include <hal/SimDevice.h>
+#include <networktables/NTSendable.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/magnetic_field_strength.h>
+#include <units/pressure.h>
+#include <units/temperature.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+#include "frc/DigitalSource.h"
+#include "frc/SPI.h"
+
+namespace frc {
+/**
+ * Use DMA SPI to read rate, acceleration, and magnetometer data from the
+ * ADIS16448 IMU and return the robots heading relative to a starting position,
+ * AHRS, and instant measurements
+ *
+ * The ADIS16448 gyro angle outputs track the robot's heading based on the
+ * starting position. As the robot rotates the new heading is computed by
+ * integrating the rate of rotation returned by the IMU. When the class is
+ * instantiated, a short calibration routine is performed where the IMU samples
+ * the gyros while at rest to determine the initial offset. This is subtracted
+ * from each sample to determine the heading.
+ *
+ * This class is for the ADIS16448 IMU connected via the SPI port available on
+ * the RoboRIO MXP port.
+ */
+
+class ADIS16448_IMU : public nt::NTSendable,
+                      public wpi::SendableHelper<ADIS16448_IMU> {
+ public:
+  /* ADIS16448 Calibration Time Enum Class */
+  enum class CalibrationTime {
+    _32ms = 0,
+    _64ms = 1,
+    _128ms = 2,
+    _256ms = 3,
+    _512ms = 4,
+    _1s = 5,
+    _2s = 6,
+    _4s = 7,
+    _8s = 8,
+    _16s = 9,
+    _32s = 10,
+    _64s = 11
+  };
+
+  enum IMUAxis { kX, kY, kZ };
+
+  /**
+   * IMU constructor on onboard MXP CS0, Z-up orientation, and complementary
+   * AHRS computation.
+   */
+  ADIS16448_IMU();
+
+  /**
+   * IMU constructor on the specified MXP port and orientation.
+   *
+   * @param yaw_axis The axis where gravity is present. Valid options are kX,
+   * kY, and kZ
+   * @param port The SPI port where the IMU is connected.
+   * @param cal_time The calibration time that should be used on start-up.
+   */
+  explicit ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
+                         CalibrationTime cal_time);
+
+  ~ADIS16448_IMU() override;
+
+  ADIS16448_IMU(ADIS16448_IMU&&) = default;
+  ADIS16448_IMU& operator=(ADIS16448_IMU&&) = default;
+
+  /**
+   * Initialize the IMU.
+   *
+   * Perform gyro offset calibration by collecting data for a number of seconds
+   * and computing the center value. The center value is subtracted from
+   * subsequent measurements.
+   *
+   * It's important to make sure that the robot is not moving while the
+   * centering calculations are in progress, this is typically done when the
+   * robot is first turned on while it's sitting at rest before the match
+   * starts.
+   *
+   * The calibration routine can be triggered by the user during runtime.
+   */
+  void Calibrate();
+
+  /**
+   * Configures the calibration time used for the next calibrate.
+   *
+   * @param new_cal_time The calibration time that should be used
+   */
+  int ConfigCalTime(CalibrationTime new_cal_time);
+
+  /**
+   * Reset the gyro.
+   *
+   * Resets the gyro accumulations to a heading of zero. This can be used if
+   * there is significant drift in the gyro and it needs to be recalibrated
+   * after running.
+   */
+  void Reset();
+
+  /**
+   * Returns the yaw axis angle in degrees (CCW positive).
+   */
+  units::degree_t GetAngle() const;
+
+  /**
+   * Returns the yaw axis angular rate in degrees per second (CCW positive).
+   */
+  units::degrees_per_second_t GetRate() const;
+
+  /**
+   * Returns the accumulated gyro angle in the X axis.
+   */
+  units::degree_t GetGyroAngleX() const;
+
+  /**
+   * Returns the accumulated gyro angle in the Y axis.
+   */
+  units::degree_t GetGyroAngleY() const;
+
+  /**
+   * Returns the accumulated gyro angle in the Z axis.
+   */
+  units::degree_t GetGyroAngleZ() const;
+
+  /**
+   * Returns the angular rate in the X axis.
+   */
+  units::degrees_per_second_t GetGyroRateX() const;
+
+  /**
+   * Returns the angular rate in the Y axis.
+   */
+  units::degrees_per_second_t GetGyroRateY() const;
+
+  /**
+   * Returns the angular rate in the Z axis.
+   */
+  units::degrees_per_second_t GetGyroRateZ() const;
+
+  /**
+   * Returns the acceleration in the X axis.
+   */
+  units::meters_per_second_squared_t GetAccelX() const;
+
+  /**
+   * Returns the acceleration in the Y axis.
+   */
+  units::meters_per_second_squared_t GetAccelY() const;
+
+  /**
+   * Returns the acceleration in the Z axis.
+   */
+  units::meters_per_second_squared_t GetAccelZ() const;
+
+  units::degree_t GetXComplementaryAngle() const;
+
+  units::degree_t GetYComplementaryAngle() const;
+
+  units::degree_t GetXFilteredAccelAngle() const;
+
+  units::degree_t GetYFilteredAccelAngle() const;
+
+  units::tesla_t GetMagneticFieldX() const;
+
+  units::tesla_t GetMagneticFieldY() const;
+
+  units::tesla_t GetMagneticFieldZ() const;
+
+  units::pounds_per_square_inch_t GetBarometricPressure() const;
+
+  units::celsius_t GetTemperature() const;
+
+  IMUAxis GetYawAxis() const;
+
+  int SetYawAxis(IMUAxis yaw_axis);
+
+  int ConfigDecRate(uint16_t DecimationRate);
+
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  int GetPort() const;
+
+  void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+  /** @brief ADIS16448 Register Map Declaration */
+  static constexpr uint8_t FLASH_CNT = 0x00;  // Flash memory write count
+  static constexpr uint8_t XGYRO_OUT = 0x04;  // X-axis gyroscope output
+  static constexpr uint8_t YGYRO_OUT = 0x06;  // Y-axis gyroscope output
+  static constexpr uint8_t ZGYRO_OUT = 0x08;  // Z-axis gyroscope output
+  static constexpr uint8_t XACCL_OUT = 0x0A;  // X-axis accelerometer output
+  static constexpr uint8_t YACCL_OUT = 0x0C;  // Y-axis accelerometer output
+  static constexpr uint8_t ZACCL_OUT = 0x0E;  // Z-axis accelerometer output
+  static constexpr uint8_t XMAGN_OUT = 0x10;  // X-axis magnetometer output
+  static constexpr uint8_t YMAGN_OUT = 0x12;  // Y-axis magnetometer output
+  static constexpr uint8_t ZMAGN_OUT = 0x14;  // Z-axis magnetometer output
+  static constexpr uint8_t BARO_OUT =
+      0x16;  // Barometer pressure measurement, high word
+  static constexpr uint8_t TEMP_OUT = 0x18;  // Temperature output
+  static constexpr uint8_t XGYRO_OFF =
+      0x1A;  // X-axis gyroscope bias offset factor
+  static constexpr uint8_t YGYRO_OFF =
+      0x1C;  // Y-axis gyroscope bias offset factor
+  static constexpr uint8_t ZGYRO_OFF =
+      0x1E;  // Z-axis gyroscope bias offset factor
+  static constexpr uint8_t XACCL_OFF =
+      0x20;  // X-axis acceleration bias offset factor
+  static constexpr uint8_t YACCL_OFF =
+      0x22;  // Y-axis acceleration bias offset factor
+  static constexpr uint8_t ZACCL_OFF =
+      0x24;  // Z-axis acceleration bias offset factor
+  static constexpr uint8_t XMAGN_HIC =
+      0x26;  // X-axis magnetometer, hard iron factor
+  static constexpr uint8_t YMAGN_HIC =
+      0x28;  // Y-axis magnetometer, hard iron factor
+  static constexpr uint8_t ZMAGN_HIC =
+      0x2A;  // Z-axis magnetometer, hard iron factor
+  static constexpr uint8_t XMAGN_SIC =
+      0x2C;  // X-axis magnetometer, soft iron factor
+  static constexpr uint8_t YMAGN_SIC =
+      0x2E;  // Y-axis magnetometer, soft iron factor
+  static constexpr uint8_t ZMAGN_SIC =
+      0x30;  // Z-axis magnetometer, soft iron factor
+  static constexpr uint8_t GPIO_CTRL = 0x32;  // GPIO control
+  static constexpr uint8_t MSC_CTRL = 0x34;   // MISC control
+  static constexpr uint8_t SMPL_PRD =
+      0x36;  // Sample clock/Decimation filter control
+  static constexpr uint8_t SENS_AVG = 0x38;    // Digital filter control
+  static constexpr uint8_t SEQ_CNT = 0x3A;     // MAGN_OUT and BARO_OUT counter
+  static constexpr uint8_t DIAG_STAT = 0x3C;   // System status
+  static constexpr uint8_t GLOB_CMD = 0x3E;    // System command
+  static constexpr uint8_t ALM_MAG1 = 0x40;    // Alarm 1 amplitude threshold
+  static constexpr uint8_t ALM_MAG2 = 0x42;    // Alarm 2 amplitude threshold
+  static constexpr uint8_t ALM_SMPL1 = 0x44;   // Alarm 1 sample size
+  static constexpr uint8_t ALM_SMPL2 = 0x46;   // Alarm 2 sample size
+  static constexpr uint8_t ALM_CTRL = 0x48;    // Alarm control
+  static constexpr uint8_t LOT_ID1 = 0x52;     // Lot identification number
+  static constexpr uint8_t LOT_ID2 = 0x54;     // Lot identification number
+  static constexpr uint8_t PROD_ID = 0x56;     // Product identifier
+  static constexpr uint8_t SERIAL_NUM = 0x58;  // Lot-specific serial number
+
+  /** @brief ADIS16448 Static Constants */
+  static constexpr double rad_to_deg = 57.2957795;
+  static constexpr double deg_to_rad = 0.0174532;
+  static constexpr double grav = 9.81;
+
+  /** @brief struct to store offset data */
+  struct OffsetData {
+    double gyro_rate_x = 0.0;
+    double gyro_rate_y = 0.0;
+    double gyro_rate_z = 0.0;
+  };
+
+  bool SwitchToStandardSPI();
+
+  bool SwitchToAutoSPI();
+
+  uint16_t ReadRegister(uint8_t reg);
+
+  void WriteRegister(uint8_t reg, uint16_t val);
+
+  void Acquire();
+
+  void Close();
+
+  // User-specified yaw axis
+  IMUAxis m_yaw_axis;
+
+  // Last read values (post-scaling)
+  double m_gyro_rate_x = 0.0;
+  double m_gyro_rate_y = 0.0;
+  double m_gyro_rate_z = 0.0;
+  double m_accel_x = 0.0;
+  double m_accel_y = 0.0;
+  double m_accel_z = 0.0;
+  double m_mag_x = 0.0;
+  double m_mag_y = 0.0;
+  double m_mag_z = 0.0;
+  double m_baro = 0.0;
+  double m_temp = 0.0;
+
+  // Complementary filter variables
+  double m_tau = 0.5;
+  double m_dt, m_alpha = 0.0;
+  double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
+
+  // vector for storing most recent imu values
+  OffsetData* m_offset_buffer = nullptr;
+
+  double m_gyro_rate_offset_x = 0.0;
+  double m_gyro_rate_offset_y = 0.0;
+  double m_gyro_rate_offset_z = 0.0;
+
+  // function to re-init offset buffer
+  void InitOffsetBuffer(int size);
+
+  // Accumulated gyro values (for offset calculation)
+  int m_avg_size = 0;
+  int m_accum_count = 0;
+
+  // Integrated gyro values
+  double m_integ_gyro_angle_x = 0.0;
+  double m_integ_gyro_angle_y = 0.0;
+  double m_integ_gyro_angle_z = 0.0;
+
+  // Complementary filter functions
+  double FormatFastConverge(double compAngle, double accAngle);
+
+  double FormatRange0to2PI(double compAngle);
+
+  double FormatAccelRange(double accelAngle, double accelZ);
+
+  double CompFilterProcess(double compAngle, double accelAngle, double omega);
+
+  // State and resource variables
+  volatile bool m_thread_active = false;
+  volatile bool m_first_run = true;
+  volatile bool m_thread_idle = false;
+  volatile bool m_start_up_mode = true;
+
+  bool m_auto_configured = false;
+  SPI::Port m_spi_port;
+  CalibrationTime m_calibration_time{0};
+  SPI* m_spi = nullptr;
+  DigitalInput* m_auto_interrupt = nullptr;
+
+  std::thread m_acquire_task;
+
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simGyroAngleX;
+  hal::SimDouble m_simGyroAngleY;
+  hal::SimDouble m_simGyroAngleZ;
+  hal::SimDouble m_simGyroRateX;
+  hal::SimDouble m_simGyroRateY;
+  hal::SimDouble m_simGyroRateZ;
+  hal::SimDouble m_simAccelX;
+  hal::SimDouble m_simAccelY;
+  hal::SimDouble m_simAccelZ;
+
+  struct NonMovableMutexWrapper {
+    wpi::mutex mutex;
+    NonMovableMutexWrapper() = default;
+
+    NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
+    NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
+
+    NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
+    NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
+      return *this;
+    }
+
+    void lock() { mutex.lock(); }
+
+    void unlock() { mutex.unlock(); }
+
+    bool try_lock() noexcept { return mutex.try_lock(); }
+  };
+
+  mutable NonMovableMutexWrapper m_mutex;
+
+  // CRC-16 Look-Up Table
+  static constexpr uint16_t adiscrc[256] = {
+      0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF, 0x1F3F,
+      0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890, 0x1E3D, 0x09F3,
+      0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992, 0x0102, 0x16CC, 0x0EDD,
+      0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD, 0x1C39, 0x0BF7, 0x13E6, 0x0428,
+      0x0387, 0x1449, 0x0C58, 0x1B96, 0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8,
+      0x0B76, 0x1367, 0x04A9, 0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74,
+      0x1265, 0x05AB, 0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A,
+      0x1A94, 0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
+      0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1, 0x060C,
+      0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3, 0x1933, 0x0EFD,
+      0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C, 0x0408, 0x13C6, 0x0BD7,
+      0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7, 0x1B37, 0x0CF9, 0x14E8, 0x0326,
+      0x0489, 0x1347, 0x0B56, 0x1C98, 0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B,
+      0x1245, 0x0A54, 0x1D9A, 0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A,
+      0x156B, 0x02A5, 0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040,
+      0x178E, 0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
+      0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3, 0x1123,
+      0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C, 0x0C18, 0x1BD6,
+      0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7, 0x1327, 0x04E9, 0x1CF8,
+      0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488, 0x1225, 0x05EB, 0x1DFA, 0x0A34,
+      0x0D9B, 0x1A55, 0x0244, 0x158A, 0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4,
+      0x056A, 0x1D7B, 0x0AB5, 0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060,
+      0x1871, 0x0FBF, 0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E,
+      0x1080, 0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
+      0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD, 0x1429,
+      0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386, 0x0B16, 0x1CD8,
+      0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9, 0x0A14, 0x1DDA, 0x05CB,
+      0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB, 0x152B, 0x02E5, 0x1AF4, 0x0D3A,
+      0x0A95, 0x1D5B, 0x054A, 0x1284};
+};
+
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
new file mode 100644
index 0000000..e6f3ce7
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
@@ -0,0 +1,417 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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-2020 Analog Devices Inc. All Rights Reserved.           */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*                                                                            */
+/* Juan Chong - frcsupport@analog.com                                         */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+#include <atomic>
+#include <memory>
+#include <thread>
+
+#include <hal/SimDevice.h>
+#include <networktables/NTSendable.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+#include "frc/DigitalSource.h"
+#include "frc/SPI.h"
+
+namespace frc {
+/**
+ * Use DMA SPI to read rate and acceleration data from the ADIS16470 IMU and
+ * return the robot's heading relative to a starting position and instant
+ * measurements
+ *
+ * The ADIS16470 gyro angle outputs track the robot's heading based on the
+ * starting position. As the robot rotates the new heading is computed by
+ * integrating the rate of rotation returned by the IMU. When the class is
+ * instantiated, a short calibration routine is performed where the IMU samples
+ * the gyros while at rest to determine the initial offset. This is subtracted
+ * from each sample to determine the heading.
+ *
+ * This class is for the ADIS16470 IMU connected via the primary SPI port
+ * available on the RoboRIO.
+ */
+
+class ADIS16470_IMU : public nt::NTSendable,
+                      public wpi::SendableHelper<ADIS16470_IMU> {
+ public:
+  /* ADIS16470 Calibration Time Enum Class */
+  enum class CalibrationTime {
+    _32ms = 0,
+    _64ms = 1,
+    _128ms = 2,
+    _256ms = 3,
+    _512ms = 4,
+    _1s = 5,
+    _2s = 6,
+    _4s = 7,
+    _8s = 8,
+    _16s = 9,
+    _32s = 10,
+    _64s = 11
+  };
+
+  enum IMUAxis { kX, kY, kZ };
+
+  /**
+   * @brief Default constructor. Uses CS0 on the 10-pin SPI port, the yaw axis
+   * is set to the IMU Z axis, and calibration time is defaulted to 4 seconds.
+   */
+  ADIS16470_IMU();
+
+  /**
+   * @brief Customizable constructor. Allows the SPI port and CS to be
+   * customized, the yaw axis used for GetAngle() is adjustable, and initial
+   * calibration time can be modified.
+   *
+   * @param yaw_axis Selects the "default" axis to use for GetAngle() and
+   * GetRate()
+   *
+   * @param port The SPI port and CS where the IMU is connected.
+   *
+   * @param cal_time The calibration time that should be used on start-up.
+   */
+  explicit ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
+                         CalibrationTime cal_time);
+
+  /**
+   * @brief Destructor. Kills the acquisiton loop and closes the SPI peripheral.
+   */
+  ~ADIS16470_IMU() override;
+
+  ADIS16470_IMU(ADIS16470_IMU&&) = default;
+  ADIS16470_IMU& operator=(ADIS16470_IMU&&) = default;
+
+  int ConfigDecRate(uint16_t reg);
+
+  /**
+   * @brief Switches the active SPI port to standard SPI mode, writes the
+   * command to activate the new null configuration, and re-enables auto SPI.
+   */
+  void Calibrate();
+
+  /**
+   * @brief Switches the active SPI port to standard SPI mode, writes a new
+   * value to the NULL_CNFG register in the IMU, and re-enables auto SPI.
+   */
+  int ConfigCalTime(CalibrationTime new_cal_time);
+
+  /**
+   * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
+   *
+   * Resets the gyro accumulations to a heading of zero. This can be used if
+   * the "zero" orientation of the sensor needs to be changed in runtime.
+   */
+  void Reset();
+
+  /**
+   * Returns the yaw axis angle in degrees (CCW positive).
+   */
+  units::degree_t GetAngle() const;
+
+  /**
+   * Returns the yaw axis angular rate in degrees per second (CCW positive).
+   */
+  units::degrees_per_second_t GetRate() const;
+
+  /**
+   * Returns the acceleration in the X axis.
+   */
+  units::meters_per_second_squared_t GetAccelX() const;
+
+  /**
+   * Returns the acceleration in the Y axis.
+   */
+  units::meters_per_second_squared_t GetAccelY() const;
+
+  /**
+   * Returns the acceleration in the Z axis.
+   */
+  units::meters_per_second_squared_t GetAccelZ() const;
+
+  units::degree_t GetXComplementaryAngle() const;
+
+  units::degree_t GetYComplementaryAngle() const;
+
+  units::degree_t GetXFilteredAccelAngle() const;
+
+  units::degree_t GetYFilteredAccelAngle() const;
+
+  IMUAxis GetYawAxis() const;
+
+  int SetYawAxis(IMUAxis yaw_axis);
+
+  // IMU yaw axis
+  IMUAxis m_yaw_axis;
+
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  int GetPort() const;
+
+  void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+  /* ADIS16470 Register Map Declaration */
+  static constexpr uint8_t FLASH_CNT = 0x00;  // Flash memory write count
+  static constexpr uint8_t DIAG_STAT =
+      0x02;  // Diagnostic and operational status
+  static constexpr uint8_t X_GYRO_LOW =
+      0x04;  // X-axis gyroscope output, lower word
+  static constexpr uint8_t X_GYRO_OUT =
+      0x06;  // X-axis gyroscope output, upper word
+  static constexpr uint8_t Y_GYRO_LOW =
+      0x08;  // Y-axis gyroscope output, lower word
+  static constexpr uint8_t Y_GYRO_OUT =
+      0x0A;  // Y-axis gyroscope output, upper word
+  static constexpr uint8_t Z_GYRO_LOW =
+      0x0C;  // Z-axis gyroscope output, lower word
+  static constexpr uint8_t Z_GYRO_OUT =
+      0x0E;  // Z-axis gyroscope output, upper word
+  static constexpr uint8_t X_ACCL_LOW =
+      0x10;  // X-axis accelerometer output, lower word
+  static constexpr uint8_t X_ACCL_OUT =
+      0x12;  // X-axis accelerometer output, upper word
+  static constexpr uint8_t Y_ACCL_LOW =
+      0x14;  // Y-axis accelerometer output, lower word
+  static constexpr uint8_t Y_ACCL_OUT =
+      0x16;  // Y-axis accelerometer output, upper word
+  static constexpr uint8_t Z_ACCL_LOW =
+      0x18;  // Z-axis accelerometer output, lower word
+  static constexpr uint8_t Z_ACCL_OUT =
+      0x1A;  // Z-axis accelerometer output, upper word
+  static constexpr uint8_t TEMP_OUT =
+      0x1C;  // Temperature output (internal, not calibrated)
+  static constexpr uint8_t TIME_STAMP = 0x1E;  // PPS mode time stamp
+  static constexpr uint8_t X_DELTANG_LOW =
+      0x24;  // X-axis delta angle output, lower word
+  static constexpr uint8_t X_DELTANG_OUT =
+      0x26;  // X-axis delta angle output, upper word
+  static constexpr uint8_t Y_DELTANG_LOW =
+      0x28;  // Y-axis delta angle output, lower word
+  static constexpr uint8_t Y_DELTANG_OUT =
+      0x2A;  // Y-axis delta angle output, upper word
+  static constexpr uint8_t Z_DELTANG_LOW =
+      0x2C;  // Z-axis delta angle output, lower word
+  static constexpr uint8_t Z_DELTANG_OUT =
+      0x2E;  // Z-axis delta angle output, upper word
+  static constexpr uint8_t X_DELTVEL_LOW =
+      0x30;  // X-axis delta velocity output, lower word
+  static constexpr uint8_t X_DELTVEL_OUT =
+      0x32;  // X-axis delta velocity output, upper word
+  static constexpr uint8_t Y_DELTVEL_LOW =
+      0x34;  // Y-axis delta velocity output, lower word
+  static constexpr uint8_t Y_DELTVEL_OUT =
+      0x36;  // Y-axis delta velocity output, upper word
+  static constexpr uint8_t Z_DELTVEL_LOW =
+      0x38;  // Z-axis delta velocity output, lower word
+  static constexpr uint8_t Z_DELTVEL_OUT =
+      0x3A;  // Z-axis delta velocity output, upper word
+  static constexpr uint8_t XG_BIAS_LOW =
+      0x40;  // X-axis gyroscope bias offset correction, lower word
+  static constexpr uint8_t XG_BIAS_HIGH =
+      0x42;  // X-axis gyroscope bias offset correction, upper word
+  static constexpr uint8_t YG_BIAS_LOW =
+      0x44;  // Y-axis gyroscope bias offset correction, lower word
+  static constexpr uint8_t YG_BIAS_HIGH =
+      0x46;  // Y-axis gyroscope bias offset correction, upper word
+  static constexpr uint8_t ZG_BIAS_LOW =
+      0x48;  // Z-axis gyroscope bias offset correction, lower word
+  static constexpr uint8_t ZG_BIAS_HIGH =
+      0x4A;  // Z-axis gyroscope bias offset correction, upper word
+  static constexpr uint8_t XA_BIAS_LOW =
+      0x4C;  // X-axis accelerometer bias offset correction, lower word
+  static constexpr uint8_t XA_BIAS_HIGH =
+      0x4E;  // X-axis accelerometer bias offset correction, upper word
+  static constexpr uint8_t YA_BIAS_LOW =
+      0x50;  // Y-axis accelerometer bias offset correction, lower word
+  static constexpr uint8_t YA_BIAS_HIGH =
+      0x52;  // Y-axis accelerometer bias offset correction, upper word
+  static constexpr uint8_t ZA_BIAS_LOW =
+      0x54;  // Z-axis accelerometer bias offset correction, lower word
+  static constexpr uint8_t ZA_BIAS_HIGH =
+      0x56;  // Z-axis accelerometer bias offset correction, upper word
+  static constexpr uint8_t FILT_CTRL = 0x5C;  // Filter control
+  static constexpr uint8_t MSC_CTRL = 0x60;   // Miscellaneous control
+  static constexpr uint8_t UP_SCALE = 0x62;   // Clock scale factor, PPS mode
+  static constexpr uint8_t DEC_RATE =
+      0x64;  // Decimation rate control (output data rate)
+  static constexpr uint8_t NULL_CNFG = 0x66;  // Auto-null configuration control
+  static constexpr uint8_t GLOB_CMD = 0x68;   // Global commands
+  static constexpr uint8_t FIRM_REV = 0x6C;   // Firmware revision
+  static constexpr uint8_t FIRM_DM =
+      0x6E;  // Firmware revision date, month and day
+  static constexpr uint8_t FIRM_Y = 0x70;   // Firmware revision date, year
+  static constexpr uint8_t PROD_ID = 0x72;  // Product identification
+  static constexpr uint8_t SERIAL_NUM =
+      0x74;  // Serial number (relative to assembly lot)
+  static constexpr uint8_t USER_SCR1 = 0x76;  // User scratch register 1
+  static constexpr uint8_t USER_SCR2 = 0x78;  // User scratch register 2
+  static constexpr uint8_t USER_SCR3 = 0x7A;  // User scratch register 3
+  static constexpr uint8_t FLSHCNT_LOW =
+      0x7C;  // Flash update count, lower word
+  static constexpr uint8_t FLSHCNT_HIGH =
+      0x7E;  // Flash update count, upper word
+
+  /* ADIS16470 Auto SPI Data Packets */
+  static constexpr uint8_t m_autospi_x_packet[16] = {
+      X_DELTANG_OUT, FLASH_CNT, X_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+      Y_GYRO_OUT,    FLASH_CNT, Z_GYRO_OUT,    FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+      Y_ACCL_OUT,    FLASH_CNT, Z_ACCL_OUT,    FLASH_CNT};
+
+  static constexpr uint8_t m_autospi_y_packet[16] = {
+      Y_DELTANG_OUT, FLASH_CNT, Y_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+      Y_GYRO_OUT,    FLASH_CNT, Z_GYRO_OUT,    FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+      Y_ACCL_OUT,    FLASH_CNT, Z_ACCL_OUT,    FLASH_CNT};
+
+  static constexpr uint8_t m_autospi_z_packet[16] = {
+      Z_DELTANG_OUT, FLASH_CNT, Z_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+      Y_GYRO_OUT,    FLASH_CNT, Z_GYRO_OUT,    FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+      Y_ACCL_OUT,    FLASH_CNT, Z_ACCL_OUT,    FLASH_CNT};
+
+  /* ADIS16470 Constants */
+  static constexpr double delta_angle_sf =
+      2160.0 / 2147483648.0; /* 2160 / (2^31) */
+  static constexpr double rad_to_deg = 57.2957795;
+  static constexpr double deg_to_rad = 0.0174532;
+  static constexpr double grav = 9.81;
+
+  /**
+   * @brief Switches to standard SPI operation. Primarily used when exiting auto
+   * SPI mode.
+   *
+   * @return A boolean indicating the success or failure of setting up the SPI
+   * peripheral in standard SPI mode.
+   */
+  bool SwitchToStandardSPI();
+
+  /**
+   * @brief Switches to auto SPI operation. Primarily used when exiting standard
+   * SPI mode.
+   *
+   * @return A boolean indicating the success or failure of setting up the SPI
+   * peripheral in auto SPI mode.
+   */
+  bool SwitchToAutoSPI();
+
+  /**
+   * @brief Reads the contents of a specified register location over SPI.
+   *
+   * @param reg An unsigned, 8-bit register location.
+   *
+   * @return An unsigned, 16-bit number representing the contents of the
+   * requested register location.
+   */
+  uint16_t ReadRegister(uint8_t reg);
+
+  /**
+   * @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
+   * locations over SPI.
+   *
+   * @param reg An unsigned, 8-bit register location.
+   *
+   * @param val An unsigned, 16-bit value to be written to the specified
+   * register location.
+   */
+  void WriteRegister(uint8_t reg, uint16_t val);
+
+  /**
+   * @brief Main acquisition loop. Typically called asynchronously and
+   * free-wheels while the robot code is active.
+   */
+  void Acquire();
+
+  void Close();
+
+  // Integrated gyro value
+  double m_integ_angle = 0.0;
+
+  // Instant raw outputs
+  double m_gyro_rate_x = 0.0;
+  double m_gyro_rate_y = 0.0;
+  double m_gyro_rate_z = 0.0;
+  double m_accel_x = 0.0;
+  double m_accel_y = 0.0;
+  double m_accel_z = 0.0;
+
+  // Complementary filter variables
+  double m_tau = 1.0;
+  double m_dt, m_alpha = 0.0;
+  double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
+
+  // Complementary filter functions
+  double FormatFastConverge(double compAngle, double accAngle);
+
+  double FormatRange0to2PI(double compAngle);
+
+  double FormatAccelRange(double accelAngle, double accelZ);
+
+  double CompFilterProcess(double compAngle, double accelAngle, double omega);
+
+  // State and resource variables
+  volatile bool m_thread_active = false;
+  volatile bool m_first_run = true;
+  volatile bool m_thread_idle = false;
+  bool m_auto_configured = false;
+  SPI::Port m_spi_port;
+  uint16_t m_calibration_time = 0;
+  SPI* m_spi = nullptr;
+  DigitalInput* m_auto_interrupt = nullptr;
+  double m_scaled_sample_rate = 2500.0;  // Default sample rate setting
+
+  std::thread m_acquire_task;
+
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simGyroAngleX;
+  hal::SimDouble m_simGyroAngleY;
+  hal::SimDouble m_simGyroAngleZ;
+  hal::SimDouble m_simGyroRateX;
+  hal::SimDouble m_simGyroRateY;
+  hal::SimDouble m_simGyroRateZ;
+  hal::SimDouble m_simAccelX;
+  hal::SimDouble m_simAccelY;
+  hal::SimDouble m_simAccelZ;
+
+  struct NonMovableMutexWrapper {
+    wpi::mutex mutex;
+    NonMovableMutexWrapper() = default;
+
+    NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
+    NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
+
+    NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
+    NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
+      return *this;
+    }
+
+    void lock() { mutex.lock(); }
+
+    void unlock() { mutex.unlock(); }
+
+    bool try_lock() noexcept { return mutex.try_lock(); }
+  };
+
+  mutable NonMovableMutexWrapper m_mutex;
+};
+
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Compressor.h b/wpilibc/src/main/native/include/frc/Compressor.h
index cc065ff..d8125ed 100644
--- a/wpilibc/src/main/native/include/frc/Compressor.h
+++ b/wpilibc/src/main/native/include/frc/Compressor.h
@@ -7,9 +7,11 @@
 #include <memory>
 
 #include <hal/Types.h>
+#include <wpi/deprecated.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
 
+#include "frc/CompressorConfigType.h"
 #include "frc/PneumaticsBase.h"
 #include "frc/PneumaticsModuleType.h"
 #include "frc/SensorUtil.h"
@@ -59,13 +61,19 @@
   /**
    * Starts closed-loop control. Note that closed loop control is enabled by
    * default.
+   *
+   * @deprecated Use EnableDigital() instead.
    */
+  WPI_DEPRECATED("Use EnableDigital() instead")
   void Start();
 
   /**
    * Stops closed-loop control. Note that closed loop control is enabled by
    * default.
+   *
+   * @deprecated Use Disable() instead.
    */
+  WPI_DEPRECATED("Use Disable() instead")
   void Stop();
 
   /**
@@ -87,25 +95,58 @@
    *
    * @return The current through the compressor, in amps
    */
-  double GetCompressorCurrent() const;
+  units::ampere_t GetCurrent() const;
 
   /**
-   * Enables or disables automatically turning the compressor on when the
-   * pressure is low.
+   * Query the analog input voltage (on channel 0) (if supported).
    *
-   * @param on Set to true to enable closed loop control of the compressor.
-   *           False to disable.
+   * @return The analog input voltage, in volts
    */
-  void SetClosedLoopControl(bool on);
+  units::volt_t GetAnalogVoltage() const;
 
   /**
-   * Returns true if the compressor will automatically turn on when the
-   * pressure is low.
+   * Query the analog sensor pressure (on channel 0) (if supported). Note this
+   * is only for use with the REV Analog Pressure Sensor.
    *
-   * @return True if closed loop control of the compressor is enabled. False if
-   *         disabled.
+   * @return The analog sensor pressure, in PSI
    */
-  bool GetClosedLoopControl() const;
+  units::pounds_per_square_inch_t GetPressure() const;
+
+  /**
+   * Disable the compressor.
+   */
+  void Disable();
+
+  /**
+   * Enable compressor closed loop control using digital input.
+   */
+  void EnableDigital();
+
+  /**
+   * Enable compressor closed loop control using analog input. Note this is only
+   * for use with the REV Analog Pressure Sensor.
+   *
+   * <p>On CTRE PCM, this will enable digital control.
+   *
+   * @param minPressure The minimum pressure in PSI to enable compressor
+   * @param maxPressure The maximum pressure in PSI to disable compressor
+   */
+  void EnableAnalog(units::pounds_per_square_inch_t minPressure,
+                    units::pounds_per_square_inch_t maxPressure);
+
+  /**
+   * Enable compressor closed loop control using hybrid input. Note this is only
+   * for use with the REV Analog Pressure Sensor.
+   *
+   * On CTRE PCM, this will enable digital control.
+   *
+   * @param minPressure The minimum pressure in PSI to enable compressor
+   * @param maxPressure The maximum pressure in PSI to disable compressor
+   */
+  void EnableHybrid(units::pounds_per_square_inch_t minPressure,
+                    units::pounds_per_square_inch_t maxPressure);
+
+  CompressorConfigType GetConfigType() const;
 
   void InitSendable(wpi::SendableBuilder& builder) override;
 
diff --git a/wpilibc/src/main/native/include/frc/CompressorConfigType.h b/wpilibc/src/main/native/include/frc/CompressorConfigType.h
new file mode 100644
index 0000000..5a5a1c1
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/CompressorConfigType.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 frc {
+enum class CompressorConfigType {
+  Disabled = 0,
+  Digital = 1,
+  Analog = 2,
+  Hybrid = 3
+};
+
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Debouncer.h b/wpilibc/src/main/native/include/frc/Debouncer.h
deleted file mode 100644
index 8f583f0..0000000
--- a/wpilibc/src/main/native/include/frc/Debouncer.h
+++ /dev/null
@@ -1,46 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can 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/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
index ccf56ad..ab7ded9 100644
--- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
+++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
@@ -122,6 +122,21 @@
   units::turn_t Get() const;
 
   /**
+   * Set the encoder duty cycle range. As the encoder needs to maintain a duty
+   * cycle, the duty cycle cannot go all the way to 0% or all the way to 100%.
+   * For example, an encoder with a 4096 us period might have a minimum duty
+   * cycle of 1 us / 4096 us and a maximum duty cycle of 4095 / 4096 us. Setting
+   * the range will result in an encoder duty cycle less than or equal to the
+   * minimum being output as 0 rotation, the duty cycle greater than or equal to
+   * the maximum being output as 1 rotation, and values in between linearly
+   * scaled from 0 to 1.
+   *
+   * @param min minimum duty cycle (0-1 range)
+   * @param max maximum duty cycle (0-1 range)
+   */
+  void SetDutyCycleRange(double min, double max);
+
+  /**
    * 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
@@ -175,6 +190,8 @@
   double m_positionOffset = 0;
   double m_distancePerRotation = 1.0;
   mutable units::turn_t m_lastPosition{0.0};
+  double m_sensorMin = 0;
+  double m_sensorMax = 1;
 
   hal::SimDevice m_simDevice;
   hal::SimDouble m_simPosition;
diff --git a/wpilibc/src/main/native/include/frc/MotorSafety.h b/wpilibc/src/main/native/include/frc/MotorSafety.h
index f84af17..1bfd34a 100644
--- a/wpilibc/src/main/native/include/frc/MotorSafety.h
+++ b/wpilibc/src/main/native/include/frc/MotorSafety.h
@@ -91,6 +91,12 @@
   static void CheckMotors();
 
   virtual void StopMotor() = 0;
+
+  /**
+   * The return value from this method is printed out when an error occurs
+   *
+   * This method must not throw!
+   */
   virtual std::string GetDescription() const = 0;
 
  private:
diff --git a/wpilibc/src/main/native/include/frc/PneumaticHub.h b/wpilibc/src/main/native/include/frc/PneumaticHub.h
index 857ed51..d412bb6 100644
--- a/wpilibc/src/main/native/include/frc/PneumaticHub.h
+++ b/wpilibc/src/main/native/include/frc/PneumaticHub.h
@@ -22,13 +22,23 @@
 
   bool GetCompressor() const override;
 
-  void SetClosedLoopControl(bool enabled) override;
+  void DisableCompressor() override;
 
-  bool GetClosedLoopControl() const override;
+  void EnableCompressorDigital() override;
+
+  void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
+
+  void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
+
+  CompressorConfigType GetCompressorConfigType() const override;
 
   bool GetPressureSwitch() const override;
 
-  double GetCompressorCurrent() const override;
+  units::ampere_t GetCompressorCurrent() const override;
 
   void SetSolenoids(int mask, int values) override;
 
@@ -57,6 +67,70 @@
                                     int reverseChannel) override;
   Compressor MakeCompressor() override;
 
+  struct Version {
+    uint32_t FirmwareMajor;
+    uint32_t FirmwareMinor;
+    uint32_t FirmwareFix;
+    uint32_t HardwareMinor;
+    uint32_t HardwareMajor;
+    uint32_t UniqueId;
+  };
+
+  Version GetVersion() const;
+
+  struct Faults {
+    uint32_t Channel0Fault : 1;
+    uint32_t Channel1Fault : 1;
+    uint32_t Channel2Fault : 1;
+    uint32_t Channel3Fault : 1;
+    uint32_t Channel4Fault : 1;
+    uint32_t Channel5Fault : 1;
+    uint32_t Channel6Fault : 1;
+    uint32_t Channel7Fault : 1;
+    uint32_t Channel8Fault : 1;
+    uint32_t Channel9Fault : 1;
+    uint32_t Channel10Fault : 1;
+    uint32_t Channel11Fault : 1;
+    uint32_t Channel12Fault : 1;
+    uint32_t Channel13Fault : 1;
+    uint32_t Channel14Fault : 1;
+    uint32_t Channel15Fault : 1;
+    uint32_t CompressorOverCurrent : 1;
+    uint32_t CompressorOpen : 1;
+    uint32_t SolenoidOverCurrent : 1;
+    uint32_t Brownout : 1;
+    uint32_t CanWarning : 1;
+    uint32_t HardwareFault : 1;
+  };
+
+  Faults GetFaults() const;
+
+  struct StickyFaults {
+    uint32_t CompressorOverCurrent : 1;
+    uint32_t CompressorOpen : 1;
+    uint32_t SolenoidOverCurrent : 1;
+    uint32_t Brownout : 1;
+    uint32_t CanWarning : 1;
+    uint32_t CanBusOff : 1;
+    uint32_t HasReset : 1;
+  };
+
+  StickyFaults GetStickyFaults() const;
+
+  void ClearStickyFaults();
+
+  units::volt_t GetInputVoltage() const;
+
+  units::volt_t Get5VRegulatedVoltage() const;
+
+  units::ampere_t GetSolenoidsTotalCurrent() const;
+
+  units::volt_t GetSolenoidsVoltage() const;
+
+  units::volt_t GetAnalogVoltage(int channel) const override;
+
+  units::pounds_per_square_inch_t GetPressure(int channel) const override;
+
  private:
   class DataStore;
   friend class DataStore;
diff --git a/wpilibc/src/main/native/include/frc/PneumaticsBase.h b/wpilibc/src/main/native/include/frc/PneumaticsBase.h
index 06cad5d..50ceb87 100644
--- a/wpilibc/src/main/native/include/frc/PneumaticsBase.h
+++ b/wpilibc/src/main/native/include/frc/PneumaticsBase.h
@@ -6,8 +6,12 @@
 
 #include <memory>
 
+#include <units/current.h>
+#include <units/pressure.h>
 #include <units/time.h>
+#include <units/voltage.h>
 
+#include "frc/CompressorConfigType.h"
 #include "frc/PneumaticsModuleType.h"
 
 namespace frc {
@@ -22,11 +26,21 @@
 
   virtual bool GetPressureSwitch() const = 0;
 
-  virtual double GetCompressorCurrent() const = 0;
+  virtual units::ampere_t GetCompressorCurrent() const = 0;
 
-  virtual void SetClosedLoopControl(bool on) = 0;
+  virtual void DisableCompressor() = 0;
 
-  virtual bool GetClosedLoopControl() const = 0;
+  virtual void EnableCompressorDigital() = 0;
+
+  virtual void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) = 0;
+
+  virtual void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) = 0;
+
+  virtual CompressorConfigType GetCompressorConfigType() const = 0;
 
   virtual void SetSolenoids(int mask, int values) = 0;
 
@@ -50,6 +64,10 @@
 
   virtual void UnreserveCompressor() = 0;
 
+  virtual units::volt_t GetAnalogVoltage(int channel) const = 0;
+
+  virtual units::pounds_per_square_inch_t GetPressure(int channel) const = 0;
+
   virtual Solenoid MakeSolenoid(int channel) = 0;
   virtual DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
                                             int reverseChannel) = 0;
diff --git a/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h b/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h
index 64686d6..ea4517b 100644
--- a/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h
+++ b/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h
@@ -22,13 +22,23 @@
 
   bool GetCompressor() const override;
 
-  void SetClosedLoopControl(bool enabled) override;
+  void DisableCompressor() override;
 
-  bool GetClosedLoopControl() const override;
+  void EnableCompressorDigital() override;
+
+  void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
+
+  void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
+
+  CompressorConfigType GetCompressorConfigType() const override;
 
   bool GetPressureSwitch() const override;
 
-  double GetCompressorCurrent() const override;
+  units::ampere_t GetCompressorCurrent() const override;
 
   bool GetCompressorCurrentTooHighFault() const;
   bool GetCompressorCurrentTooHighStickyFault() const;
@@ -64,6 +74,10 @@
 
   void UnreserveCompressor() override;
 
+  units::volt_t GetAnalogVoltage(int channel) const override;
+
+  units::pounds_per_square_inch_t GetPressure(int channel) const override;
+
   Solenoid MakeSolenoid(int channel) override;
   DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
                                     int reverseChannel) override;
diff --git a/wpilibc/src/main/native/include/frc/PowerDistribution.h b/wpilibc/src/main/native/include/frc/PowerDistribution.h
index 2863daf..763e6bf 100644
--- a/wpilibc/src/main/native/include/frc/PowerDistribution.h
+++ b/wpilibc/src/main/native/include/frc/PowerDistribution.h
@@ -12,25 +12,26 @@
 
 /**
  * Class for getting voltage, current, temperature, power and energy from the
- * CAN PDP.
+ * CTRE Power Distribution Panel (PDP) or REV Power Distribution Hub (PDH).
  */
 class PowerDistribution : public wpi::Sendable,
                           public wpi::SendableHelper<PowerDistribution> {
  public:
   static constexpr int kDefaultModule = -1;
-  enum class ModuleType { kAutomatic = 0, kCTRE = 1, kRev = 2 };
+  enum class ModuleType { kCTRE = 1, kRev = 2 };
 
   /**
-   * Constructs a PowerDistribution.
+   * Constructs a PowerDistribution object.
    *
-   * Uses the default CAN ID.
+   * Detects the connected PDP/PDH using the default CAN ID (0 for CTRE and 1
+   * for REV).
    */
   PowerDistribution();
 
   /**
-   * Constructs a PowerDistribution.
+   * Constructs a PowerDistribution object.
    *
-   * @param module The CAN ID of the PDP
+   * @param module The CAN ID of the PDP/PDH
    * @param moduleType The type of module
    */
   PowerDistribution(int module, ModuleType moduleType);
@@ -40,56 +41,57 @@
   PowerDistribution& operator=(PowerDistribution&&) = default;
 
   /**
-   * Query the input voltage of the PDP.
+   * Query the input voltage of the PDP/PDH.
    *
-   * @return The voltage of the PDP in volts
+   * @return The input voltage in volts
    */
   double GetVoltage() const;
 
   /**
-   * Query the temperature of the PDP.
+   * Query the temperature of the PDP/PDH.
    *
-   * @return The temperature of the PDP in degrees Celsius
+   * @return The temperature in degrees Celsius
    */
   double GetTemperature() const;
 
   /**
-   * Query the current of a single channel of the PDP.
+   * Query the current of a single channel of the PDP/PDH.
    *
-   * @return The current of one of the PDP channels (channels 0-15) in Amperes
+   * @param channel the channel to query (0-15 for PDP, 0-23 for PDH)
+   * @return The current of the channel in Amperes
    */
   double GetCurrent(int channel) const;
 
   /**
-   * Query the total current of all monitored PDP channels (0-15).
+   * Query the total current of all monitored PDP/PDH channels.
    *
-   * @return The the total current drawn from the PDP channels in Amperes
+   * @return The total current drawn from all channels in Amperes
    */
   double GetTotalCurrent() const;
 
   /**
-   * Query the total power drawn from the monitored PDP channels.
+   * Query the total power drawn from all monitored PDP/PDH channels.
    *
-   * @return The the total power drawn from the PDP channels in Watts
+   * @return The total power drawn in Watts
    */
   double GetTotalPower() const;
 
   /**
-   * Query the total energy drawn from the monitored PDP channels.
+   * Query the total energy drawn from the monitored PDP/PDH channels.
    *
-   * @return The the total energy drawn from the PDP channels in Joules
+   * @return The total energy drawn in Joules
    */
   double GetTotalEnergy() const;
 
   /**
-   * Reset the total energy drawn from the PDP.
+   * Reset the total energy drawn from the PDP/PDH.
    *
    * @see PowerDistribution#GetTotalEnergy
    */
   void ResetTotalEnergy();
 
   /**
-   * Remove all of the fault flags on the PDP.
+   * Remove all of the fault flags on the PDP/PDH.
    */
   void ClearStickyFaults();
 
@@ -98,10 +100,102 @@
    */
   int GetModule() const;
 
+  /**
+   * Gets module type.
+   */
+  ModuleType GetType() const;
+
+  /**
+   * Gets whether the PDH switchable channel is turned on or off. Returns false
+   * with the CTRE PDP.
+   *
+   * @return The output state of the PDH switchable channel
+   */
   bool GetSwitchableChannel() const;
 
+  /**
+   * Sets the PDH switchable channel on or off. Does nothing with the CTRE PDP.
+   *
+   * @param enabled Whether to turn the PDH switchable channel on or off
+   */
   void SetSwitchableChannel(bool enabled);
 
+  struct Version {
+    uint32_t FirmwareMajor;
+    uint32_t FirmwareMinor;
+    uint32_t FirmwareFix;
+    uint32_t HardwareMinor;
+    uint32_t HardwareMajor;
+    uint32_t UniqueId;
+  };
+
+  Version GetVersion() const;
+
+  struct Faults {
+    uint32_t Channel0BreakerFault : 1;
+    uint32_t Channel1BreakerFault : 1;
+    uint32_t Channel2BreakerFault : 1;
+    uint32_t Channel3BreakerFault : 1;
+    uint32_t Channel4BreakerFault : 1;
+    uint32_t Channel5BreakerFault : 1;
+    uint32_t Channel6BreakerFault : 1;
+    uint32_t Channel7BreakerFault : 1;
+    uint32_t Channel8BreakerFault : 1;
+    uint32_t Channel9BreakerFault : 1;
+    uint32_t Channel10BreakerFault : 1;
+    uint32_t Channel11BreakerFault : 1;
+    uint32_t Channel12BreakerFault : 1;
+    uint32_t Channel13BreakerFault : 1;
+    uint32_t Channel14BreakerFault : 1;
+    uint32_t Channel15BreakerFault : 1;
+    uint32_t Channel16BreakerFault : 1;
+    uint32_t Channel17BreakerFault : 1;
+    uint32_t Channel18BreakerFault : 1;
+    uint32_t Channel19BreakerFault : 1;
+    uint32_t Channel20BreakerFault : 1;
+    uint32_t Channel21BreakerFault : 1;
+    uint32_t Channel22BreakerFault : 1;
+    uint32_t Channel23BreakerFault : 1;
+    uint32_t Brownout : 1;
+    uint32_t CanWarning : 1;
+    uint32_t HardwareFault : 1;
+  };
+
+  Faults GetFaults() const;
+
+  struct StickyFaults {
+    uint32_t Channel0BreakerFault : 1;
+    uint32_t Channel1BreakerFault : 1;
+    uint32_t Channel2BreakerFault : 1;
+    uint32_t Channel3BreakerFault : 1;
+    uint32_t Channel4BreakerFault : 1;
+    uint32_t Channel5BreakerFault : 1;
+    uint32_t Channel6BreakerFault : 1;
+    uint32_t Channel7BreakerFault : 1;
+    uint32_t Channel8BreakerFault : 1;
+    uint32_t Channel9BreakerFault : 1;
+    uint32_t Channel10BreakerFault : 1;
+    uint32_t Channel11BreakerFault : 1;
+    uint32_t Channel12BreakerFault : 1;
+    uint32_t Channel13BreakerFault : 1;
+    uint32_t Channel14BreakerFault : 1;
+    uint32_t Channel15BreakerFault : 1;
+    uint32_t Channel16BreakerFault : 1;
+    uint32_t Channel17BreakerFault : 1;
+    uint32_t Channel18BreakerFault : 1;
+    uint32_t Channel19BreakerFault : 1;
+    uint32_t Channel20BreakerFault : 1;
+    uint32_t Channel21BreakerFault : 1;
+    uint32_t Channel22BreakerFault : 1;
+    uint32_t Channel23BreakerFault : 1;
+    uint32_t Brownout : 1;
+    uint32_t CanWarning : 1;
+    uint32_t CanBusOff : 1;
+    uint32_t HasReset : 1;
+  };
+
+  StickyFaults GetStickyFaults() const;
+
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
diff --git a/wpilibc/src/main/native/include/frc/SPI.h b/wpilibc/src/main/native/include/frc/SPI.h
index 36c82e4..4f12cbe 100644
--- a/wpilibc/src/main/native/include/frc/SPI.h
+++ b/wpilibc/src/main/native/include/frc/SPI.h
@@ -35,7 +35,7 @@
    */
   explicit SPI(Port port);
 
-  ~SPI();
+  virtual ~SPI();
 
   SPI(SPI&&) = default;
   SPI& operator=(SPI&&) = default;
diff --git a/wpilibc/src/main/native/include/frc/Servo.h b/wpilibc/src/main/native/include/frc/Servo.h
index b1df655..96cbfa4 100644
--- a/wpilibc/src/main/native/include/frc/Servo.h
+++ b/wpilibc/src/main/native/include/frc/Servo.h
@@ -46,7 +46,8 @@
    * Get the servo position.
    *
    * Servo values range from 0.0 to 1.0 corresponding to the range of full left
-   * to full right.
+   * to full right. This returns the commanded position, not the position that
+   * the servo is actually at, as the servo does not report its own position.
    *
    * @return Position from 0.0 to 1.0.
    */
@@ -55,8 +56,8 @@
   /**
    * Set the servo angle.
    *
-   * Assume that the servo angle is linear with respect to the PWM value (big
-   * assumption, need to test).
+   * The angles are based on the HS-322HD Servo, and have a range of 0 to 180
+   * degrees.
    *
    * Servo angles that are out of the supported range of the servo simply
    * "saturate" in that direction. In other words, if the servo has a range of
@@ -71,8 +72,8 @@
   /**
    * Get the servo angle.
    *
-   * Assume that the servo angle is linear with respect to the PWM value (big
-   * assumption, need to test).
+   * This returns the commanded angle, not the angle that the servo is actually
+   * at, as the servo does not report its own angle.
    *
    * @return The angle in degrees to which the servo is set.
    */
diff --git a/wpilibc/src/main/native/include/frc/WPIWarnings.mac b/wpilibc/src/main/native/include/frc/WPIWarnings.mac
index fa63494..6ac2c11 100644
--- a/wpilibc/src/main/native/include/frc/WPIWarnings.mac
+++ b/wpilibc/src/main/native/include/frc/WPIWarnings.mac
@@ -10,7 +10,7 @@
 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(BadJoystickIndex, 7, "Joystick index is out of range, should be 0-5")
 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")
diff --git a/wpilibc/src/main/native/include/frc/counter/EdgeConfiguration.h b/wpilibc/src/main/native/include/frc/counter/EdgeConfiguration.h
new file mode 100644
index 0000000..508117d
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/counter/EdgeConfiguration.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 frc {
+enum class EdgeConfiguration {
+  kNone = 0,
+  kRisingEdge = 0x1,
+  kFallingEdge = 0x2,
+  kBoth = 0x3
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/counter/ExternalDirectionCounter.h b/wpilibc/src/main/native/include/frc/counter/ExternalDirectionCounter.h
new file mode 100644
index 0000000..4d9faf2
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/counter/ExternalDirectionCounter.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 <memory>
+
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "EdgeConfiguration.h"
+
+namespace frc {
+class DigitalSource;
+
+/** Counter using external direction.
+ *
+ * <p>This counts on an edge from one digital input and the whether it counts
+ * up or down based on the state of a second digital input.
+ *
+ */
+class ExternalDirectionCounter
+    : public wpi::Sendable,
+      public wpi::SendableHelper<ExternalDirectionCounter> {
+ public:
+  /**
+   * Constructs a new ExternalDirectionCounter.
+   *
+   * @param countSource The source for counting.
+   * @param directionSource The source for selecting count direction.
+   */
+  ExternalDirectionCounter(DigitalSource& countSource,
+                           DigitalSource& directionSource);
+
+  /**
+   * Constructs a new ExternalDirectionCounter.
+   *
+   * @param countSource The source for counting.
+   * @param directionSource The source for selecting count direction.
+   */
+  ExternalDirectionCounter(std::shared_ptr<DigitalSource> countSource,
+                           std::shared_ptr<DigitalSource> directionSource);
+
+  ~ExternalDirectionCounter() override;
+
+  ExternalDirectionCounter(ExternalDirectionCounter&&) = default;
+  ExternalDirectionCounter& operator=(ExternalDirectionCounter&&) = default;
+
+  /**
+   * Gets the current count.
+   *
+   * @return The current count.
+   */
+  int GetCount() const;
+
+  /**
+   * Sets to reverse the counter direction.
+   *
+   * @param reverseDirection True to reverse counting direction.
+   */
+  void SetReverseDirection(bool reverseDirection);
+
+  /** Resets the current count. */
+  void Reset();
+
+  /**
+   * Sets the edge configuration for counting.
+   *
+   * @param configuration The counting edge configuration.
+   */
+  void SetEdgeConfiguration(EdgeConfiguration configuration);
+
+ protected:
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  std::shared_ptr<DigitalSource> m_countSource;
+  std::shared_ptr<DigitalSource> m_directionSource;
+  hal::Handle<HAL_CounterHandle> m_handle;
+  int32_t m_index = 0;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/counter/Tachometer.h b/wpilibc/src/main/native/include/frc/counter/Tachometer.h
new file mode 100644
index 0000000..dba1575
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/counter/Tachometer.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 <hal/Types.h>
+#include <units/angular_velocity.h>
+#include <units/frequency.h>
+#include <units/time.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+class DigitalSource;
+
+/**
+ * Tachometer for getting rotational speed from a device.
+ *
+ * <p>The Tachometer class measures the time between digital pulses to
+ * determine the rotation speed of a mechanism. Examples of devices that could
+ * be used with the tachometer class are a hall effect sensor, break beam
+ * sensor, or optical sensor detecting tape on a shooter wheel. Unlike
+ * encoders, this class only needs a single digital input.
+ */
+class Tachometer : public wpi::Sendable,
+                   public wpi::SendableHelper<Tachometer> {
+ public:
+  /**
+   * Constructs a new tachometer.
+   *
+   * @param source The source.
+   */
+  explicit Tachometer(DigitalSource& source);
+
+  /**
+   * Constructs a new tachometer.
+   *
+   * @param source The source.
+   */
+  explicit Tachometer(std::shared_ptr<DigitalSource> source);
+
+  ~Tachometer() override;
+
+  Tachometer(Tachometer&&) = default;
+  Tachometer& operator=(Tachometer&&) = default;
+
+  /**
+   * Gets the tachometer frequency.
+   *
+   * @return Current frequency.
+   */
+  units::hertz_t GetFrequency() const;
+
+  /**
+   * Gets the tachometer period.
+   *
+   * @return Current period.
+   */
+  units::second_t GetPeriod() const;
+
+  /**
+   * Gets the number of edges per revolution.
+   *
+   * @return Edges per revolution.
+   */
+  int GetEdgesPerRevolution() const;
+
+  /**
+   * Sets the number of edges per revolution.
+   *
+   * @param edges Edges per revolution.
+   */
+  void SetEdgesPerRevolution(int edges);
+
+  /**
+   * Gets the current tachometer revolutions per second.
+   *
+   * SetEdgesPerRevolution must be set with a non 0 value for this to work.
+   *
+   * @return Current RPS.
+   */
+  units::turns_per_second_t GetRevolutionsPerSecond() const;
+
+  /**
+   * Gets the current tachometer revolutions per minute.
+   *
+   * SetEdgesPerRevolution must be set with a non 0 value for this to work.
+   *
+   * @return Current RPM.
+   */
+  units::revolutions_per_minute_t GetRevolutionsPerMinute() const;
+
+  /**
+   * Gets if the tachometer is stopped.
+   *
+   * @return True if the tachometer is stopped.
+   */
+  bool GetStopped() const;
+
+  /**
+   * Gets the number of sample to average.
+   *
+   * @return Samples to average.
+   */
+  int GetSamplesToAverage() const;
+
+  /**
+   * Sets the number of samples to average.
+   *
+   * @param samples Samples to average.
+   */
+  void SetSamplesToAverage(int samples);
+
+  /**
+   * Sets the maximum period before the tachometer is considered stopped.
+   *
+   * @param maxPeriod The max period.
+   */
+  void SetMaxPeriod(units::second_t maxPeriod);
+
+  /**
+   * Sets if to update when empty.
+   *
+   * @param updateWhenEmpty True to update when empty.
+   */
+  void SetUpdateWhenEmpty(bool updateWhenEmpty);
+
+ protected:
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  std::shared_ptr<DigitalSource> m_source;
+  hal::Handle<HAL_CounterHandle> m_handle;
+  int m_edgesPerRevolution;
+  int32_t m_index;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/counter/UpDownCounter.h b/wpilibc/src/main/native/include/frc/counter/UpDownCounter.h
new file mode 100644
index 0000000..b01ce42
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/counter/UpDownCounter.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 <memory>
+
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "EdgeConfiguration.h"
+
+namespace frc {
+class DigitalSource;
+
+/** Up Down Counter.
+ *
+ * This class can count edges on a single digital input or count up based on an
+ * edge from one digital input and down on an edge from another digital input.
+ *
+ */
+class UpDownCounter : public wpi::Sendable,
+                      public wpi::SendableHelper<UpDownCounter> {
+ public:
+  /**
+   * Constructs a new UpDown Counter.
+   *
+   * @param upSource The up count source (can be null).
+   * @param downSource The down count source (can be null).
+   */
+  UpDownCounter(DigitalSource& upSource, DigitalSource& downSource);
+
+  /**
+   * Constructs a new UpDown Counter.
+   *
+   * @param upSource The up count source (can be null).
+   * @param downSource The down count source (can be null).
+   */
+  UpDownCounter(std::shared_ptr<DigitalSource> upSource,
+                std::shared_ptr<DigitalSource> downSource);
+
+  ~UpDownCounter() override;
+
+  UpDownCounter(UpDownCounter&&) = default;
+  UpDownCounter& operator=(UpDownCounter&&) = default;
+
+  /**
+   * Gets the current count.
+   *
+   * @return The current count.
+   */
+  int GetCount() const;
+
+  /**
+   * Sets to revert the counter direction.
+   *
+   * @param reverseDirection True to reverse counting direction.
+   */
+  void SetReverseDirection(bool reverseDirection);
+
+  /** Resets the current count. */
+  void Reset();
+
+  /**
+   * Sets the configuration for the up source.
+   *
+   * @param configuration The up source configuration.
+   */
+  void SetUpEdgeConfiguration(EdgeConfiguration configuration);
+
+  /**
+   * Sets the configuration for the down source.
+   *
+   * @param configuration The down source configuration.
+   */
+  void SetDownEdgeConfiguration(EdgeConfiguration configuration);
+
+ protected:
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  void InitUpDownCounter();
+  std::shared_ptr<DigitalSource> m_upSource;
+  std::shared_ptr<DigitalSource> m_downSource;
+  hal::Handle<HAL_CounterHandle> m_handle;
+  int32_t m_index = 0;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
index 50a7a3f..7009eb9 100644
--- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
@@ -101,6 +101,11 @@
                           public wpi::Sendable,
                           public wpi::SendableHelper<DifferentialDrive> {
  public:
+  /**
+   * Wheel speeds for a differential drive.
+   *
+   * Uses normalized voltage [-1.0..1.0].
+   */
   struct WheelSpeeds {
     double left = 0.0;
     double right = 0.0;
@@ -142,10 +147,11 @@
    *
    * @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 zRotation        The normalized curvature [-1.0..1.0]. Clockwise is
+   *                         positive.
    * @param allowTurnInPlace If set, overrides constant-curvature turning for
-   *                         turn-in-place maneuvers.
+   *                         turn-in-place maneuvers. zRotation will control
+   *                         turning rate instead of curvature.
    */
   void CurvatureDrive(double xSpeed, double zRotation, bool allowTurnInPlace);
 
@@ -171,6 +177,7 @@
    * @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.
+   * @return Wheel speeds [-1.0..1.0].
    */
   static WheelSpeeds ArcadeDriveIK(double xSpeed, double zRotation,
                                    bool squareInputs = true);
@@ -184,10 +191,12 @@
    *
    * @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 zRotation        The normalized curvature [-1.0..1.0]. Clockwise is
+   *                         positive.
    * @param allowTurnInPlace If set, overrides constant-curvature turning for
-   *                         turn-in-place maneuvers.
+   *                         turn-in-place maneuvers. zRotation will control
+   *                         turning rate instead of curvature.
+   * @return Wheel speeds [-1.0..1.0].
    */
   static WheelSpeeds CurvatureDriveIK(double xSpeed, double zRotation,
                                       bool allowTurnInPlace);
@@ -200,6 +209,7 @@
    * @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 [-1.0..1.0].
    */
   static WheelSpeeds TankDriveIK(double leftSpeed, double rightSpeed,
                                  bool squareInputs = true);
diff --git a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h b/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
index 1acc69f..d4c5c5c 100644
--- a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
@@ -62,6 +62,11 @@
   static constexpr double kDefaultRightMotorAngle = 120.0;
   static constexpr double kDefaultBackMotorAngle = 270.0;
 
+  /**
+   * Wheel speeds for a Killough drive.
+   *
+   * Uses normalized voltage [-1.0..1.0].
+   */
   struct WheelSpeeds {
     double left = 0.0;
     double right = 0.0;
@@ -154,6 +159,7 @@
    *                  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 [-1.0..1.0].
    */
   WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed, double zRotation,
                                double gyroAngle = 0.0);
diff --git a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
index 809e7ea..757ae67 100644
--- a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
@@ -44,25 +44,20 @@
  * </pre>
  *
  * Each Drive() function provides different inverse kinematic relations for a
- * Mecanum drive robot. Motor outputs for the right side are negated, so motor
- * direction inversion by the user is usually unnecessary.
+ * Mecanum drive robot.
  *
- * This library uses the NED axes convention (North-East-Down as external
- * reference in the world frame):
- * http://www.nuclearprojects.com/ins/images/axis_big.png.
- *
- * The positive X axis points ahead, the positive Y axis points to the right,
+ * The positive Y axis points ahead, the positive X axis points to the right,
  * and the positive Z axis points down. Rotations follow the right-hand rule, so
  * clockwise rotation around the Z axis is positive.
  *
+ * Note: the axis conventions used in this class differ from DifferentialDrive.
+ * This may change in a future year's WPILib release.
+ *
  * 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().
  *
  * RobotDrive porting guide:
- * <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's MecanumDrive_Cartesian(double, double, double, double)
  * if a deadband of 0 is used, and the ySpeed and gyroAngle values are inverted
@@ -76,6 +71,11 @@
                      public wpi::Sendable,
                      public wpi::SendableHelper<MecanumDrive> {
  public:
+  /**
+   * Wheel speeds for a mecanum drive.
+   *
+   * Uses normalized voltage [-1.0..1.0].
+   */
   struct WheelSpeeds {
     double frontLeft = 0.0;
     double frontRight = 0.0;
@@ -103,9 +103,9 @@
    * 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
+   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Forward is
    *                  positive.
-   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is
+   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Right is
    *                  positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
    *                  Clockwise is positive.
@@ -136,14 +136,15 @@
    * 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
+   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Forward is
    *                  positive.
-   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is
+   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Right 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 [-1.0..1.0].
    */
   static WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed,
                                       double zRotation, double gyroAngle = 0.0);
diff --git a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
index 389d67b..8ce5636 100644
--- a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
+++ b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
@@ -82,10 +82,10 @@
   static double ApplyDeadband(double value, double deadband);
 
   /**
-   * Normalize all wheel speeds if the magnitude of any wheel is greater than
+   * Renormalize all wheel speeds if the magnitude of any wheel is greater than
    * 1.0.
    */
-  static void Normalize(wpi::span<double> wheelSpeeds);
+  static void Desaturate(wpi::span<double> wheelSpeeds);
 
   double m_deadband = 0.02;
   double m_maxOutput = 1.0;
diff --git a/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h b/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h
index ac6ba4b..7bb63fb 100644
--- a/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h
+++ b/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h
@@ -61,6 +61,16 @@
 
   int GetChannel() const;
 
+  /**
+   * Optionally eliminate the deadband from a motor controller.
+   *
+   * @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);
+
  protected:
   /**
    * Constructor for a PWM Motor %Controller connected via PWM.
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h b/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
index 9ac315f..d7ed1d5 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
@@ -370,7 +370,16 @@
    * </td></tr>
    * </table>
    */
-  kCameraStream
+  kCameraStream,
+  /**
+   * Displays a field2d object.<br>
+   * Supported types:
+   *
+   * <ul>
+   *   <li>Field2d
+   * </ul>
+   */
+  kField,
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h b/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
index e7c9a81..4792ce3 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
@@ -7,9 +7,13 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 
 #ifndef DYNAMIC_CAMERA_SERVER
 #include <cscore_oo.h>
+#include <fmt/format.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
 #else
 namespace cs {
 class VideoSource;
@@ -20,6 +24,7 @@
 
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
+#include <wpi/span.h>
 
 namespace frc {
 
@@ -28,8 +33,8 @@
 namespace detail {
 constexpr const char* kProtocol = "camera_server://";
 std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
-    CS_Source source);
-void AddToSendableRegistry(wpi::Sendable* sendable, std::string name);
+    std::string_view cameraName);
+void AddToSendableRegistry(wpi::Sendable* sendable, std::string_view name);
 }  // namespace detail
 
 /**
@@ -46,9 +51,9 @@
    * 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
+   * @param cameraName the name of the camera to wrap
    */
-  SendableCameraWrapper(CS_Source source, const private_init&);
+  SendableCameraWrapper(std::string_view cameraName, const private_init&);
 
   /**
    * Gets a sendable wrapper object for the given video source, creating the
@@ -61,6 +66,9 @@
   static SendableCameraWrapper& Wrap(const cs::VideoSource& source);
   static SendableCameraWrapper& Wrap(CS_Source source);
 
+  static SendableCameraWrapper& Wrap(std::string_view cameraName,
+                                     wpi::span<const std::string> cameraUrls);
+
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
@@ -68,11 +76,9 @@
 };
 
 #ifndef DYNAMIC_CAMERA_SERVER
-inline SendableCameraWrapper::SendableCameraWrapper(CS_Source source,
+inline SendableCameraWrapper::SendableCameraWrapper(std::string_view name,
                                                     const private_init&)
     : m_uri(detail::kProtocol) {
-  CS_Status status = 0;
-  auto name = cs::GetSourceName(source, &status);
   detail::AddToSendableRegistry(this, name);
   m_uri += name;
 }
@@ -83,12 +89,27 @@
 }
 
 inline SendableCameraWrapper& SendableCameraWrapper::Wrap(CS_Source source) {
-  auto& wrapper = detail::GetSendableCameraWrapper(source);
+  CS_Status status = 0;
+  auto name = cs::GetSourceName(source, &status);
+  auto& wrapper = detail::GetSendableCameraWrapper(name);
   if (!wrapper) {
-    wrapper = std::make_shared<SendableCameraWrapper>(source, private_init{});
+    wrapper = std::make_shared<SendableCameraWrapper>(name, private_init{});
   }
   return *wrapper;
 }
+
+inline SendableCameraWrapper& SendableCameraWrapper::Wrap(
+    std::string_view cameraName, wpi::span<const std::string> cameraUrls) {
+  auto& wrapper = detail::GetSendableCameraWrapper(cameraName);
+  if (!wrapper) {
+    wrapper =
+        std::make_shared<SendableCameraWrapper>(cameraName, private_init{});
+  }
+  auto streams = fmt::format("/CameraPublisher/{}/streams", cameraName);
+  nt::NetworkTableInstance::GetDefault().GetEntry(streams).SetStringArray(
+      cameraUrls);
+  return *wrapper;
+}
 #endif
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
index db58a21..13a5cee 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
@@ -128,6 +128,18 @@
   ComplexWidget& Add(std::string_view title, const cs::VideoSource& video);
 
   /**
+   * Adds a widget to this container to display a video stream.
+   *
+   * @param title      the title of the widget
+   * @param cameraName the name of the streamed camera
+   * @param cameraUrls the URLs with which the dashboard can access the camera
+   * stream
+   * @return a widget to display the camera stream
+   */
+  ComplexWidget& AddCamera(std::string_view title, std::string_view cameraName,
+                           wpi::span<const std::string> cameraUrls);
+
+  /**
    * Adds a widget to this container to display the given sendable.
    *
    * @param sendable the sendable to display
@@ -526,4 +538,10 @@
     std::string_view title, const cs::VideoSource& video) {
   return Add(title, frc::SendableCameraWrapper::Wrap(video));
 }
+
+inline frc::ComplexWidget& frc::ShuffleboardContainer::AddCamera(
+    std::string_view title, std::string_view cameraName,
+    wpi::span<const std::string> cameraUrls) {
+  return Add(title, frc::SendableCameraWrapper::Wrap(cameraName, cameraUrls));
+}
 #endif
diff --git a/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h b/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h
new file mode 100644
index 0000000..9618695
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.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 <hal/SimDevice.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+namespace frc {
+
+class ADIS16448_IMU;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADIS16448 IMU.
+ */
+class ADIS16448_IMUSim {
+ public:
+  /**
+   * Constructs from a ADIS16448_IMU object.
+   *
+   * @param imu ADIS16448_IMU to simulate
+   */
+  explicit ADIS16448_IMUSim(const ADIS16448_IMU& imu);
+
+  /**
+   * Sets the X axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleX(units::degree_t angle);
+
+  /**
+   * Sets the Y axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleY(units::degree_t angle);
+
+  /**
+   * Sets the Z axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleZ(units::degree_t angle);
+
+  /**
+   * Sets the X axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateX(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the Y axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateY(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the Z axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateZ(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the X axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelX(units::meters_per_second_squared_t accel);
+
+  /**
+   * Sets the Y axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelY(units::meters_per_second_squared_t accel);
+
+  /**
+   * Sets the Z axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelZ(units::meters_per_second_squared_t accel);
+
+ private:
+  hal::SimDouble m_simGyroAngleX;
+  hal::SimDouble m_simGyroAngleY;
+  hal::SimDouble m_simGyroAngleZ;
+  hal::SimDouble m_simGyroRateX;
+  hal::SimDouble m_simGyroRateY;
+  hal::SimDouble m_simGyroRateZ;
+  hal::SimDouble m_simAccelX;
+  hal::SimDouble m_simAccelY;
+  hal::SimDouble m_simAccelZ;
+};
+
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h b/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h
new file mode 100644
index 0000000..7b43762
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.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 <hal/SimDevice.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+namespace frc {
+
+class ADIS16470_IMU;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADIS16470 IMU.
+ */
+class ADIS16470_IMUSim {
+ public:
+  /**
+   * Constructs from a ADIS16470_IMU object.
+   *
+   * @param imu ADIS16470_IMU to simulate
+   */
+  explicit ADIS16470_IMUSim(const ADIS16470_IMU& imu);
+
+  /**
+   * Sets the X axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleX(units::degree_t angle);
+
+  /**
+   * Sets the Y axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleY(units::degree_t angle);
+
+  /**
+   * Sets the Z axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleZ(units::degree_t angle);
+
+  /**
+   * Sets the X axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateX(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the Y axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateY(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the Z axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateZ(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the X axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelX(units::meters_per_second_squared_t accel);
+
+  /**
+   * Sets the Y axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelY(units::meters_per_second_squared_t accel);
+
+  /**
+   * Sets the Z axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelZ(units::meters_per_second_squared_t accel);
+
+ private:
+  hal::SimDouble m_simGyroAngleX;
+  hal::SimDouble m_simGyroAngleY;
+  hal::SimDouble m_simGyroAngleZ;
+  hal::SimDouble m_simGyroRateX;
+  hal::SimDouble m_simGyroRateY;
+  hal::SimDouble m_simGyroRateZ;
+  hal::SimDouble m_simAccelX;
+  hal::SimDouble m_simAccelY;
+  hal::SimDouble m_simAccelZ;
+};
+
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h
new file mode 100644
index 0000000..b1388bd
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.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 <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/moment_of_inertia.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated DC motor mechanism.
+ */
+class DCMotorSim : public LinearSystemSim<2, 1, 2> {
+ public:
+  /**
+   * Creates a simulated DC motor mechanism.
+   *
+   * @param plant              The linear system representing the DC motor.
+   * @param gearbox            The type of and number of motors in the DC motor
+   * gearbox.
+   * @param gearing            The gearing of the DC motor (numbers greater than
+   * 1 represent reductions).
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   */
+  DCMotorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox,
+             double gearing,
+             const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
+
+  /**
+   * Creates a simulated DC motor mechanism.
+   *
+   * @param gearbox            The type of and number of motors in the DC motor
+   * gearbox.
+   * @param gearing            The gearing of the DC motor (numbers greater than
+   * 1 represent reductions).
+   * @param moi                The moment of inertia of the DC motor.
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   */
+  DCMotorSim(const DCMotor& gearbox, double gearing,
+             units::kilogram_square_meter_t moi,
+             const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
+
+  /**
+   * Returns the DC motor position.
+   *
+   * @return The DC motor position.
+   */
+  units::radian_t GetAngularPosition() const;
+
+  /**
+   * Returns the DC motor velocity.
+   *
+   * @return The DC motor velocity.
+   */
+  units::radians_per_second_t GetAngularVelocity() const;
+
+  /**
+   * Returns the DC motor current draw.
+   *
+   * @return The DC motor current draw.
+   */
+  units::ampere_t GetCurrentDraw() const override;
+
+  /**
+   * Sets the input voltage for the DC motor.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+ private:
+  DCMotor m_gearbox;
+  double m_gearing;
+};
+}  // namespace frc::sim
diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
index e64a652..7c53eb7 100644
--- a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
@@ -225,6 +225,12 @@
     static constexpr frc::DCMotor SingleMiniCIMPerSide =
         frc::DCMotor::MiniCIM(1);
     static constexpr frc::DCMotor DualMiniCIMPerSide = frc::DCMotor::MiniCIM(2);
+    static constexpr frc::DCMotor SingleFalcon500PerSide =
+        frc::DCMotor::Falcon500(1);
+    static constexpr frc::DCMotor DualFalcon500PerSide =
+        frc::DCMotor::Falcon500(2);
+    static constexpr frc::DCMotor SingleNEOPerSide = frc::DCMotor::NEO(1);
+    static constexpr frc::DCMotor DualNEOPerSide = frc::DCMotor::NEO(2);
   };
 
   class KitbotWheelSize {
diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
index 61cbd46..731fad3 100644
--- a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
@@ -45,6 +45,8 @@
     m_u = Eigen::Vector<double, Inputs>::Zero();
   }
 
+  virtual ~LinearSystemSim() = default;
+
   /**
    * Updates the simulation.
    *
@@ -136,7 +138,7 @@
    * @return The normalized input.
    */
   Eigen::Vector<double, Inputs> ClampInput(Eigen::Vector<double, Inputs> u) {
-    return frc::NormalizeInputVector<Inputs>(
+    return frc::DesaturateInputVector<Inputs>(
         u, frc::RobotController::GetInputVoltage());
   }
 
diff --git a/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h b/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h
index 97e31bf..e5f4efe 100644
--- a/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h
@@ -119,22 +119,22 @@
    * @return the CallbackStore object associated with this callback
    */
   [[nodiscard]] std::unique_ptr<CallbackStore>
-  RegisterClosedLoopEnabledCallback(NotifyCallback callback,
-                                    bool initialNotify);
+  RegisterCompressorConfigTypeCallback(NotifyCallback callback,
+                                       bool initialNotify);
 
   /**
    * Check whether the closed loop compressor control is active.
    *
-   * @return true if active
+   * @return compressor config type
    */
-  bool GetClosedLoopEnabled() const;
+  int GetCompressorConfigType() const;
 
   /**
    * Turn on/off the closed loop control of the compressor.
    *
-   * @param closedLoopEnabled whether the control loop is active
+   * @param compressorConfigType compressor config type
    */
-  void SetClosedLoopEnabled(bool closedLoopEnabled);
+  void SetCompressorConfigType(int compressorConfigType);
 
   /**
    * Register a callback to be run whenever the pressure switch value changes.
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h
index 70abaa6..f1bebb9 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h
@@ -34,7 +34,14 @@
    *
    * @param color the color of the line
    */
-  void SetColor(const frc::Color8Bit& color);
+  void SetColor(const Color8Bit& color);
+
+  /**
+   * Get the ligament color.
+   *
+   * @return the color of the line
+   */
+  Color8Bit GetColor();
 
   /**
    * Set the ligament's length.
@@ -71,6 +78,13 @@
    */
   void SetLineWeight(double lineWidth);
 
+  /**
+   * Get the line thickness.
+   *
+   * @return the line thickness
+   */
+  double GetLineWeight();
+
  protected:
   void UpdateEntries(std::shared_ptr<nt::NetworkTable> table) override;
 
diff --git a/wpilibc/src/test/native/cpp/DebouncerTest.cpp b/wpilibc/src/test/native/cpp/DebouncerTest.cpp
deleted file mode 100644
index 379a6d5..0000000
--- a/wpilibc/src/test/native/cpp/DebouncerTest.cpp
+++ /dev/null
@@ -1,48 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you 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/wpilibc/src/test/native/cpp/TestCallbackHelpers.cpp b/wpilibc/src/test/native/cpp/TestCallbackHelpers.cpp
index d98f539..12ae5d2 100644
--- a/wpilibc/src/test/native/cpp/TestCallbackHelpers.cpp
+++ b/wpilibc/src/test/native/cpp/TestCallbackHelpers.cpp
@@ -4,8 +4,6 @@
 
 #include "callback_helpers/TestCallbackHelpers.h"
 
-#include <iostream>
-
 #include <fmt/format.h>
 
 namespace frc::sim {
diff --git a/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
index 1d13a77..b930346 100644
--- a/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
+++ b/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
@@ -6,6 +6,238 @@
 #include "frc/drive/DifferentialDrive.h"
 #include "gtest/gtest.h"
 
+TEST(DifferentialDriveTest, ArcadeDriveIK) {
+  // Forward
+  auto speeds = frc::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, false);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Forward left turn
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, false);
+  EXPECT_DOUBLE_EQ(0.0, speeds.left);
+  EXPECT_DOUBLE_EQ(0.5, speeds.right);
+
+  // Forward right turn
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, false);
+  EXPECT_DOUBLE_EQ(0.5, speeds.left);
+  EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+  // Backward
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, false);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+  // Backward left turn
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, false);
+  EXPECT_DOUBLE_EQ(-0.5, speeds.left);
+  EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+  // Backward right turn
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, false);
+  EXPECT_DOUBLE_EQ(0.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-0.5, speeds.right);
+
+  // Left turn (xSpeed with negative sign)
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Left turn (xSpeed with positive sign)
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Right turn (xSpeed with negative sign)
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+  // Right turn (xSpeed with positive sign)
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+}
+
+TEST(DifferentialDriveTest, ArcadeDriveIKSquared) {
+  // Forward
+  auto speeds = frc::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, true);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Forward left turn
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, true);
+  EXPECT_DOUBLE_EQ(0.0, speeds.left);
+  EXPECT_DOUBLE_EQ(0.25, speeds.right);
+
+  // Forward right turn
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, true);
+  EXPECT_DOUBLE_EQ(0.25, speeds.left);
+  EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+  // Backward
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, true);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+  // Backward left turn
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, true);
+  EXPECT_DOUBLE_EQ(-0.25, speeds.left);
+  EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+  // Backward right turn
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, true);
+  EXPECT_DOUBLE_EQ(0.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-0.25, speeds.right);
+
+  // Left turn (xSpeed with negative sign)
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Left turn (xSpeed with positive sign)
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Right turn (xSpeed with negative sign)
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+  // Right turn (xSpeed with positive sign)
+  speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+}
+
+TEST(DifferentialDriveTest, CurvatureDriveIK) {
+  // Forward
+  auto speeds = frc::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, false);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Forward left turn
+  speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, false);
+  EXPECT_DOUBLE_EQ(0.25, speeds.left);
+  EXPECT_DOUBLE_EQ(0.75, speeds.right);
+
+  // Forward right turn
+  speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, false);
+  EXPECT_DOUBLE_EQ(0.75, speeds.left);
+  EXPECT_DOUBLE_EQ(0.25, speeds.right);
+
+  // Backward
+  speeds = frc::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, false);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+  // Backward left turn
+  speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, false);
+  EXPECT_DOUBLE_EQ(-0.75, speeds.left);
+  EXPECT_DOUBLE_EQ(-0.25, speeds.right);
+
+  // Backward right turn
+  speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, false);
+  EXPECT_DOUBLE_EQ(-0.25, speeds.left);
+  EXPECT_DOUBLE_EQ(-0.75, speeds.right);
+}
+
+TEST(DifferentialDriveTest, CurvatureDriveIKTurnInPlace) {
+  // Forward
+  auto speeds = frc::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, true);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Forward left turn
+  speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, true);
+  EXPECT_DOUBLE_EQ(0.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Forward right turn
+  speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, true);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+  // Backward
+  speeds = frc::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, true);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+  // Backward left turn
+  speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, true);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+  // Backward right turn
+  speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, true);
+  EXPECT_DOUBLE_EQ(0.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+}
+
+TEST(DifferentialDriveTest, TankDriveIK) {
+  // Forward
+  auto speeds = frc::DifferentialDrive::TankDriveIK(1.0, 1.0, false);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Forward left turn
+  speeds = frc::DifferentialDrive::TankDriveIK(0.5, 1.0, false);
+  EXPECT_DOUBLE_EQ(0.5, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Forward right turn
+  speeds = frc::DifferentialDrive::TankDriveIK(1.0, 0.5, false);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(0.5, speeds.right);
+
+  // Backward
+  speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -1.0, false);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+  // Backward left turn
+  speeds = frc::DifferentialDrive::TankDriveIK(-0.5, -1.0, false);
+  EXPECT_DOUBLE_EQ(-0.5, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+  // Backward right turn
+  speeds = frc::DifferentialDrive::TankDriveIK(-0.5, 1.0, false);
+  EXPECT_DOUBLE_EQ(-0.5, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+}
+
+TEST(DifferentialDriveTest, TankDriveIKSquared) {
+  // Forward
+  auto speeds = frc::DifferentialDrive::TankDriveIK(1.0, 1.0, true);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Forward left turn
+  speeds = frc::DifferentialDrive::TankDriveIK(0.5, 1.0, true);
+  EXPECT_DOUBLE_EQ(0.25, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+  // Forward right turn
+  speeds = frc::DifferentialDrive::TankDriveIK(1.0, 0.5, true);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(0.25, speeds.right);
+
+  // Backward
+  speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -1.0, true);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+  // Backward left turn
+  speeds = frc::DifferentialDrive::TankDriveIK(-0.5, -1.0, true);
+  EXPECT_DOUBLE_EQ(-0.25, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+  // Backward right turn
+  speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -0.5, true);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-0.25, speeds.right);
+}
+
 TEST(DifferentialDriveTest, ArcadeDrive) {
   frc::MockMotorController left;
   frc::MockMotorController right;
diff --git a/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp
index 510b377..c23c7b0 100644
--- a/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp
+++ b/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp
@@ -8,6 +8,80 @@
 #include "frc/drive/KilloughDrive.h"
 #include "gtest/gtest.h"
 
+TEST(KilloughDriveTest, CartesianIK) {
+  frc::MockMotorController left;
+  frc::MockMotorController right;
+  frc::MockMotorController back;
+  frc::KilloughDrive drive{left, right, back};
+
+  // Forward
+  auto speeds = drive.DriveCartesianIK(1.0, 0.0, 0.0);
+  EXPECT_DOUBLE_EQ(0.5, speeds.left);
+  EXPECT_DOUBLE_EQ(-0.5, speeds.right);
+  EXPECT_NEAR(0.0, speeds.back, 1e-9);
+
+  // Left
+  speeds = drive.DriveCartesianIK(0.0, -1.0, 0.0);
+  EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.left);
+  EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.right);
+  EXPECT_DOUBLE_EQ(1.0, speeds.back);
+
+  // Right
+  speeds = drive.DriveCartesianIK(0.0, 1.0, 0.0);
+  EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, speeds.left);
+  EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, speeds.right);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.back);
+
+  // Rotate CCW
+  speeds = drive.DriveCartesianIK(0.0, 0.0, -1.0);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.back);
+
+  // Rotate CW
+  speeds = drive.DriveCartesianIK(0.0, 0.0, 1.0);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+  EXPECT_DOUBLE_EQ(1.0, speeds.back);
+}
+
+TEST(KilloughDriveTest, CartesianIKGyro90CW) {
+  frc::MockMotorController left;
+  frc::MockMotorController right;
+  frc::MockMotorController back;
+  frc::KilloughDrive drive{left, right, back};
+
+  // Forward in global frame; left in robot frame
+  auto speeds = drive.DriveCartesianIK(1.0, 0.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.left);
+  EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.right);
+  EXPECT_DOUBLE_EQ(1.0, speeds.back);
+
+  // Left in global frame; backward in robot frame
+  speeds = drive.DriveCartesianIK(0.0, -1.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(-0.5, speeds.left);
+  EXPECT_NEAR(0.5, speeds.right, 1e-9);
+  EXPECT_NEAR(0.0, speeds.back, 1e-9);
+
+  // Right in global frame; forward in robot frame
+  speeds = drive.DriveCartesianIK(0.0, 1.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(0.5, speeds.left);
+  EXPECT_NEAR(-0.5, speeds.right, 1e-9);
+  EXPECT_NEAR(0.0, speeds.back, 1e-9);
+
+  // Rotate CCW
+  speeds = drive.DriveCartesianIK(0.0, 0.0, -1.0, 90.0);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.back);
+
+  // Rotate CW
+  speeds = drive.DriveCartesianIK(0.0, 0.0, 1.0, 90.0);
+  EXPECT_DOUBLE_EQ(1.0, speeds.left);
+  EXPECT_DOUBLE_EQ(1.0, speeds.right);
+  EXPECT_DOUBLE_EQ(1.0, speeds.back);
+}
+
 TEST(KilloughDriveTest, Cartesian) {
   frc::MockMotorController left;
   frc::MockMotorController right;
diff --git a/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
index fe0a73b..2990848 100644
--- a/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
+++ b/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
@@ -6,12 +6,86 @@
 #include "frc/drive/MecanumDrive.h"
 #include "gtest/gtest.h"
 
+TEST(MecanumDriveTest, CartesianIK) {
+  // Forward
+  auto speeds = frc::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0);
+  EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
+  EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+  EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+  EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
+
+  // Left
+  speeds = frc::MecanumDrive::DriveCartesianIK(0.0, -1.0, 0.0);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
+  EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+  EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
+
+  // Right
+  speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 1.0, 0.0);
+  EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
+  EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
+
+  // Rotate CCW
+  speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, -1.0);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
+  EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
+  EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
+
+  // Rotate CW
+  speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, 1.0);
+  EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
+  EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
+}
+
+TEST(MecanumDriveTest, CartesianIKGyro90CW) {
+  // Forward in global frame; left in robot frame
+  auto speeds = frc::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
+  EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+  EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
+
+  // Left in global frame; backward in robot frame
+  speeds = frc::MecanumDrive::DriveCartesianIK(0.0, -1.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
+
+  // Right in global frame; forward in robot frame
+  speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 1.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
+  EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+  EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+  EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
+
+  // Rotate CCW
+  speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, -1.0, 90.0);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
+  EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
+  EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
+
+  // Rotate CW
+  speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, 1.0, 90.0);
+  EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
+  EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+  EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
+}
+
 TEST(MecanumDriveTest, Cartesian) {
   frc::MockMotorController fl;
-  frc::MockMotorController fr;
   frc::MockMotorController rl;
+  frc::MockMotorController fr;
   frc::MockMotorController rr;
-  frc::MecanumDrive drive{fl, fr, rl, rr};
+  frc::MecanumDrive drive{fl, rl, fr, rr};
   drive.SetDeadband(0.0);
 
   // Forward
@@ -38,15 +112,15 @@
   // 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, 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, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
   EXPECT_DOUBLE_EQ(-1.0, rr.Get());
 }
 
@@ -55,7 +129,7 @@
   frc::MockMotorController fr;
   frc::MockMotorController rl;
   frc::MockMotorController rr;
-  frc::MecanumDrive drive{fl, fr, rl, rr};
+  frc::MecanumDrive drive{fl, rl, fr, rr};
   drive.SetDeadband(0.0);
 
   // Forward in global frame; left in robot frame
@@ -82,15 +156,15 @@
   // 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, 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, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
   EXPECT_DOUBLE_EQ(-1.0, rr.Get());
 }
 
@@ -99,7 +173,7 @@
   frc::MockMotorController fr;
   frc::MockMotorController rl;
   frc::MockMotorController rr;
-  frc::MecanumDrive drive{fl, fr, rl, rr};
+  frc::MecanumDrive drive{fl, rl, fr, rr};
   drive.SetDeadband(0.0);
 
   // Forward
@@ -126,14 +200,14 @@
   // 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, 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, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
   EXPECT_DOUBLE_EQ(-1.0, rr.Get());
 }
diff --git a/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp
index a8f95da..8082ed5 100644
--- a/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp
@@ -96,7 +96,7 @@
   EXPECT_TRUE(callback.GetLastValue());
 }
 
-TEST(CTREPCMSimTest, SetClosedLoopEnabled) {
+TEST(CTREPCMSimTest, SetEnableDigital) {
   PneumaticsControlModule pcm;
   CTREPCMSim sim(pcm);
   sim.ResetData();
@@ -105,12 +105,12 @@
   auto cb =
       sim.RegisterClosedLoopEnabledCallback(callback.GetCallback(), false);
 
-  pcm.SetClosedLoopControl(false);
-  EXPECT_FALSE(pcm.GetClosedLoopControl());
+  pcm.DisableCompressor();
+  EXPECT_EQ(pcm.GetCompressorConfigType(), CompressorConfigType::Disabled);
 
-  pcm.SetClosedLoopControl(true);
+  pcm.EnableCompressorDigital();
   EXPECT_TRUE(sim.GetClosedLoopEnabled());
-  EXPECT_TRUE(pcm.GetClosedLoopControl());
+  EXPECT_EQ(pcm.GetCompressorConfigType(), CompressorConfigType::Digital);
   EXPECT_TRUE(callback.WasTriggered());
   EXPECT_TRUE(callback.GetLastValue());
 }
@@ -143,7 +143,7 @@
 
   sim.SetCompressorCurrent(35.04);
   EXPECT_EQ(35.04, sim.GetCompressorCurrent());
-  EXPECT_EQ(35.04, pcm.GetCompressorCurrent());
+  EXPECT_EQ(35.04_A, pcm.GetCompressorCurrent());
   EXPECT_TRUE(callback.WasTriggered());
   EXPECT_EQ(35.04, callback.GetLastValue());
 }
diff --git a/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp
new file mode 100644
index 0000000..efed939
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.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/Encoder.h"
+#include "frc/RobotController.h"
+#include "frc/controller/PIDController.h"
+#include "frc/motorcontrol/PWMVictorSPX.h"
+#include "frc/simulation/BatterySim.h"
+#include "frc/simulation/DCMotorSim.h"
+#include "frc/simulation/EncoderSim.h"
+#include "frc/simulation/RoboRioSim.h"
+#include "gtest/gtest.h"
+
+TEST(DCMotorSimTest, VoltageSteadyState) {
+  frc::DCMotor gearbox = frc::DCMotor::NEO(1);
+  frc::sim::DCMotorSim sim{gearbox, 1.0,
+                           units::kilogram_square_meter_t{0.0005}};
+
+  frc::Encoder encoder{0, 1};
+  frc::sim::EncoderSim encoderSim{encoder};
+  frc::PWMVictorSPX motor{0};
+
+  frc::sim::RoboRioSim::ResetData();
+  encoderSim.ResetData();
+
+  // Spin-up
+  for (int i = 0; i < 100; i++) {
+    // RobotPeriodic runs first
+    motor.SetVoltage(12_V);
+
+    // Then, SimulationPeriodic runs
+    frc::sim::RoboRioSim::SetVInVoltage(
+        frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
+    sim.SetInputVoltage(motor.Get() *
+                        frc::RobotController::GetBatteryVoltage());
+    sim.Update(20_ms);
+    encoderSim.SetRate(sim.GetAngularVelocity().value());
+  }
+
+  EXPECT_NEAR((gearbox.Kv * 12_V).value(), encoder.GetRate(), 0.1);
+
+  // Decay
+  for (int i = 0; i < 100; i++) {
+    // RobotPeriodic runs first
+    motor.SetVoltage(0_V);
+
+    // Then, SimulationPeriodic runs
+    frc::sim::RoboRioSim::SetVInVoltage(
+        frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
+    sim.SetInputVoltage(motor.Get() *
+                        frc::RobotController::GetBatteryVoltage());
+    sim.Update(20_ms);
+    encoderSim.SetRate(sim.GetAngularVelocity().value());
+  }
+
+  EXPECT_NEAR(0, encoder.GetRate(), 0.1);
+}
+
+TEST(DCMotorSimTest, PositionFeedbackControl) {
+  frc::DCMotor gearbox = frc::DCMotor::NEO(1);
+  frc::sim::DCMotorSim sim{gearbox, 1.0,
+                           units::kilogram_square_meter_t{0.0005}};
+
+  frc2::PIDController controller{0.04, 0.0, 0.001};
+
+  frc::Encoder encoder{0, 1};
+  frc::sim::EncoderSim encoderSim{encoder};
+  frc::PWMVictorSPX motor{0};
+
+  frc::sim::RoboRioSim::ResetData();
+  encoderSim.ResetData();
+
+  for (int i = 0; i < 140; i++) {
+    // RobotPeriodic runs first
+    motor.Set(controller.Calculate(encoder.GetDistance(), 750));
+
+    // Then, SimulationPeriodic runs
+    frc::sim::RoboRioSim::SetVInVoltage(
+        frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
+    sim.SetInputVoltage(motor.Get() *
+                        frc::RobotController::GetBatteryVoltage());
+    sim.Update(20_ms);
+    encoderSim.SetDistance(sim.GetAngularPosition().value());
+    encoderSim.SetRate(sim.GetAngularVelocity().value());
+  }
+
+  EXPECT_NEAR(encoder.GetDistance(), 750, 1.0);
+  EXPECT_NEAR(encoder.GetRate(), 0, 0.1);
+}
diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
similarity index 93%
rename from wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
rename to wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
index eabbecb..8ccfcd6 100644
--- a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
@@ -17,7 +17,7 @@
 #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
 #include "gtest/gtest.h"
 
-TEST(DifferentialDriveSimTest, Convergence) {
+TEST(DifferentialDrivetrainSimTest, 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);
@@ -42,9 +42,8 @@
   auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
       frc::Pose2d(), {}, frc::Pose2d(2_m, 2_m, 0_rad), config);
 
-  // NOLINTNEXTLINE
-  for (double t = 0; t < trajectory.TotalTime().value(); t += 0.02) {
-    auto state = trajectory.Sample(20_ms);
+  for (auto t = 0_s; t < trajectory.TotalTime(); t += 20_ms) {
+    auto state = trajectory.Sample(t);
     auto ramseteOut = ramsete.Calculate(sim.GetPose(), state);
 
     auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut);
@@ -70,7 +69,7 @@
   EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01);
 }
 
-TEST(DifferentialDriveSimTest, Current) {
+TEST(DifferentialDrivetrainSimTest, 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);
@@ -97,7 +96,7 @@
   EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
 }
 
-TEST(DifferentialDriveSimTest, ModelStability) {
+TEST(DifferentialDrivetrainSimTest, 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/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
index bfc50e1..03022ab 100644
--- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
@@ -2,8 +2,6 @@
 // Open Source Software; 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"
diff --git a/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp
index 7786b29..688f37e 100644
--- a/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp
@@ -96,23 +96,67 @@
   EXPECT_TRUE(callback.GetLastValue());
 }
 
-TEST(REVPHSimTest, SetClosedLoopEnabled) {
+TEST(REVPHSimTest, SetEnableDigital) {
   PneumaticHub ph;
   REVPHSim sim(ph);
   sim.ResetData();
 
-  BooleanCallback callback;
+  EnumCallback callback;
   auto cb =
-      sim.RegisterClosedLoopEnabledCallback(callback.GetCallback(), false);
+      sim.RegisterCompressorConfigTypeCallback(callback.GetCallback(), false);
 
-  ph.SetClosedLoopControl(false);
-  EXPECT_FALSE(ph.GetClosedLoopControl());
+  ph.DisableCompressor();
+  EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Disabled);
 
-  ph.SetClosedLoopControl(true);
-  EXPECT_TRUE(sim.GetClosedLoopEnabled());
-  EXPECT_TRUE(ph.GetClosedLoopControl());
+  ph.EnableCompressorDigital();
+  EXPECT_EQ(sim.GetCompressorConfigType(),
+            static_cast<int>(CompressorConfigType::Digital));
+  EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Digital);
   EXPECT_TRUE(callback.WasTriggered());
-  EXPECT_TRUE(callback.GetLastValue());
+  EXPECT_EQ(callback.GetLastValue(),
+            static_cast<int>(CompressorConfigType::Digital));
+}
+
+TEST(REVPHSimTest, SetEnableAnalog) {
+  PneumaticHub ph;
+  REVPHSim sim(ph);
+  sim.ResetData();
+
+  EnumCallback callback;
+  auto cb =
+      sim.RegisterCompressorConfigTypeCallback(callback.GetCallback(), false);
+
+  ph.DisableCompressor();
+  EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Disabled);
+
+  ph.EnableCompressorAnalog(1_psi, 2_psi);
+  EXPECT_EQ(sim.GetCompressorConfigType(),
+            static_cast<int>(CompressorConfigType::Analog));
+  EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Analog);
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(callback.GetLastValue(),
+            static_cast<int>(CompressorConfigType::Analog));
+}
+
+TEST(REVPHSimTest, SetEnableHybrid) {
+  PneumaticHub ph;
+  REVPHSim sim(ph);
+  sim.ResetData();
+
+  EnumCallback callback;
+  auto cb =
+      sim.RegisterCompressorConfigTypeCallback(callback.GetCallback(), false);
+
+  ph.DisableCompressor();
+  EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Disabled);
+
+  ph.EnableCompressorHybrid(1_psi, 2_psi);
+  EXPECT_EQ(sim.GetCompressorConfigType(),
+            static_cast<int>(CompressorConfigType::Hybrid));
+  EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Hybrid);
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(callback.GetLastValue(),
+            static_cast<int>(CompressorConfigType::Hybrid));
 }
 
 TEST(REVPHSimTest, SetPressureSwitchEnabled) {
@@ -143,7 +187,7 @@
 
   sim.SetCompressorCurrent(35.04);
   EXPECT_EQ(35.04, sim.GetCompressorCurrent());
-  EXPECT_EQ(35.04, ph.GetCompressorCurrent());
+  EXPECT_EQ(35.04_A, ph.GetCompressorCurrent());
   EXPECT_TRUE(callback.WasTriggered());
   EXPECT_EQ(35.04, callback.GetLastValue());
 }
diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
index e7e0405..23af83d 100644
--- a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
@@ -2,8 +2,6 @@
 // Open Source Software; 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/angular_acceleration.h>
 #include <units/angular_velocity.h>