[wpilibc] Add virtual dtor for LinearSystemSim (#3861)

The gyro rate getters were removed since that data isn't available.

.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
   "enableCppIntellisense": true,
   "currentLanguage": "cpp",
-  "projectYear": "2021",
+  "projectYear": "intellisense",
   "teamNumber": 0
 }

LICENSE.md

@@ -1,4 +1,4 @@
-Copyright (c) 2009-2021 FIRST and other WPILib contributors
+Copyright (c) 2009-2022 FIRST and other WPILib contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

MavenArtifacts.md

@@ -69,15 +69,36 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
 
 * wpiutil
 
+* wpigui
+  * imgui
+
+* ntcore
+  * wpiutil
+
+* wpimath
+  * wpiutil
+
+* glass/libglass
+  * wpiutil
+  * wpimath
+  * wpigui
+
+* glass/libglassnt
+  * wpiutil
+  * ntcore
+  * wpimath
+  * wpigui
+
 * hal
   * wpiutil
 
 * halsim
-  * imgui
-  * wpiutil
-
-* ntcore
   * wpiutil
+  * ntcore
+  * wpimath
+  * wpigui
+  * libglass
+  * libglassnt
 
 * cscore
   * opencv
@@ -101,6 +122,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * cameraserver
   * ntcore
   * cscore
+  * wpimath
   * wpiutil
 
 * wpilibNewCommands
@@ -109,9 +131,10 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * cameraserver
   * ntcore
   * cscore
+  * wpimath
   * wpiutil
 
-* wpilibNewCommands
+* wpilibOldCommands
   * wpilibc
   * hal
   * cameraserver
@@ -119,6 +142,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * cscore
   * wpiutil
 
+
 ### Third Party Artifacts
 
 This repository provides the builds of the following third party software.
@@ -128,3 +152,4 @@ All artifacts are based at `edu.wpi.first.thirdparty.frcYEAR` in the repository.
 * googletest
 * imgui
 * opencv
+* libssh

hal/src/main/native/athena/REVPDH.cpp

@@ -11,6 +11,7 @@
 #include <hal/handles/IndexedHandleResource.h>
 
 #include <cstring>
+#include <thread>
 
 #include <fmt/format.h>
 
@@ -35,6 +36,7 @@ struct REV_PDHObj {
   int32_t controlPeriod;
   HAL_CANHandle hcan;
   std::string previousAllocation;
+  HAL_PowerDistributionVersion versionInfo;
 };
 
 }  // namespace
@@ -226,6 +228,7 @@ HAL_REVPDHHandle HAL_InitializeREVPDH(int32_t module,
   hpdh->previousAllocation = allocationLocation ? allocationLocation : "";
   hpdh->hcan = hcan;
   hpdh->controlPeriod = kDefaultControlPeriod;
+  std::memset(&hpdh->versionInfo, 0, sizeof(hpdh->versionInfo));
 
   return handle;
 }
@@ -490,6 +493,18 @@ void HAL_GetREVPDHVersion(HAL_REVPDHHandle handle,
     return;
   }
 
+  if (hpdh->versionInfo.firmwareMajor > 0) {
+    version->firmwareMajor = hpdh->versionInfo.firmwareMajor;
+    version->firmwareMinor = hpdh->versionInfo.firmwareMinor;
+    version->firmwareFix = hpdh->versionInfo.firmwareFix;
+    version->hardwareMajor = hpdh->versionInfo.hardwareMajor;
+    version->hardwareMinor = hpdh->versionInfo.hardwareMinor;
+    version->uniqueId = hpdh->versionInfo.uniqueId;
+
+    *status = 0;
+    return;
+  }
+
   HAL_WriteCANRTRFrame(hpdh->hcan, PDH_VERSION_LENGTH, PDH_VERSION_FRAME_API,
                        status);
 
@@ -497,9 +512,15 @@ void HAL_GetREVPDHVersion(HAL_REVPDHHandle handle,
     return;
   }
 
-  HAL_ReadCANPacketTimeout(hpdh->hcan, PDH_VERSION_FRAME_API, packedData,
-                           &length, &timestamp, kDefaultControlPeriod * 2,
-                           status);
+  uint32_t timeoutMs = 100;
+  for (uint32_t i = 0; i <= timeoutMs; i++) {
+    HAL_ReadCANPacketNew(hpdh->hcan, PDH_VERSION_FRAME_API, packedData, &length,
+                         &timestamp, status);
+    if (*status == 0) {
+      break;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
 
   if (*status != 0) {
     return;
@@ -513,6 +534,8 @@ void HAL_GetREVPDHVersion(HAL_REVPDHHandle handle,
   version->hardwareMinor = result.hardware_minor;
   version->hardwareMajor = result.hardware_major;
   version->uniqueId = result.unique_id;
+
+  hpdh->versionInfo = *version;
 }
 
 void HAL_GetREVPDHFaults(HAL_REVPDHHandle handle,

hal/src/main/native/athena/REVPH.cpp

@@ -4,6 +4,8 @@
 
 #include "hal/REVPH.h"
 
+#include <thread>
+
 #include <fmt/format.h>
 
 #include "HALInitializer.h"
@@ -63,6 +65,7 @@ struct REV_PHObj {
   wpi::mutex solenoidLock;
   HAL_CANHandle hcan;
   std::string previousAllocation;
+  HAL_REVPHVersion versionInfo;
 };
 
 }  // namespace
@@ -221,6 +224,9 @@ HAL_REVPHHandle HAL_InitializeREVPH(int32_t module,
   hph->previousAllocation = allocationLocation ? allocationLocation : "";
   hph->hcan = hcan;
   hph->controlPeriod = kDefaultControlPeriod;
+  std::memset(&hph->desiredSolenoidsState, 0,
+              sizeof(hph->desiredSolenoidsState));
+  std::memset(&hph->versionInfo, 0, sizeof(hph->versionInfo));
 
   // Start closed-loop compressor control by starting solenoid state updates
   HAL_SendREVPHSolenoidsState(hph.get(), status);
@@ -480,6 +486,18 @@ void HAL_GetREVPHVersion(HAL_REVPHHandle handle, HAL_REVPHVersion* version,
     return;
   }
 
+  if (ph->versionInfo.firmwareMajor > 0) {
+    version->firmwareMajor = ph->versionInfo.firmwareMajor;
+    version->firmwareMinor = ph->versionInfo.firmwareMinor;
+    version->firmwareFix = ph->versionInfo.firmwareFix;
+    version->hardwareMajor = ph->versionInfo.hardwareMajor;
+    version->hardwareMinor = ph->versionInfo.hardwareMinor;
+    version->uniqueId = ph->versionInfo.uniqueId;
+
+    *status = 0;
+    return;
+  }
+
   HAL_WriteCANRTRFrame(ph->hcan, PH_VERSION_LENGTH, PH_VERSION_FRAME_API,
                        status);
 
@@ -487,8 +505,15 @@ void HAL_GetREVPHVersion(HAL_REVPHHandle handle, HAL_REVPHVersion* version,
     return;
   }
 
-  HAL_ReadCANPacketTimeout(ph->hcan, PH_VERSION_FRAME_API, packedData, &length,
-                           &timestamp, kDefaultControlPeriod * 2, status);
+  uint32_t timeoutMs = 100;
+  for (uint32_t i = 0; i <= timeoutMs; i++) {
+    HAL_ReadCANPacketNew(ph->hcan, PH_VERSION_FRAME_API, packedData, &length,
+                         &timestamp, status);
+    if (*status == 0) {
+      break;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
 
   if (*status != 0) {
     return;
@@ -502,6 +527,8 @@ void HAL_GetREVPHVersion(HAL_REVPHHandle handle, HAL_REVPHVersion* version,
   version->hardwareMinor = result.hardware_minor;
   version->hardwareMajor = result.hardware_major;
   version->uniqueId = result.unique_id;
+
+  ph->versionInfo = *version;
 }
 
 int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) {

hal/src/main/native/include/hal/Types.h

@@ -74,6 +74,11 @@ typedef int32_t HAL_Bool;
 
 #ifdef __cplusplus
 #define HAL_ENUM(name) enum name : int32_t
+#elif defined(__clang__)
+#define HAL_ENUM(name)    \
+  enum name : int32_t;    \
+  typedef enum name name; \
+  enum name : int32_t
 #else
 #define HAL_ENUM(name)  \
   typedef int32_t name; \

imgui/CMakeLists.txt.in

@@ -5,7 +5,7 @@ project(imgui-download NONE)
 include(ExternalProject)
 ExternalProject_Add(glfw3
   GIT_REPOSITORY    https://github.com/glfw/glfw.git
-  GIT_TAG           3.3.5
+  GIT_TAG           3.3.6
   SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/glfw-src"
   BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/glfw-build"
   CONFIGURE_COMMAND ""
@@ -23,7 +23,7 @@ ExternalProject_Add(gl3w
 )
 ExternalProject_Add(imgui
   GIT_REPOSITORY    https://github.com/ocornut/imgui.git
-  GIT_TAG           v1.85
+  GIT_TAG           v1.86
   SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/imgui-src"
   BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/imgui-build"
   CONFIGURE_COMMAND ""

myRobot/build.gradle

@@ -162,6 +162,7 @@ model {
                 lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
                 lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
                 lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+                lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                 lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                 lib project: ':cscore', library: 'cscore', linkage: 'shared'
                 lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
@@ -171,7 +172,6 @@ model {
                 project(':hal').addHalDependency(binary, 'shared')
                 project(':hal').addHalJniDependency(binary)
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-                lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                 if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
                     nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
                 } else {
@@ -209,11 +209,11 @@ model {
                 lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'static'
                 lib project: ':wpilibc', library: 'wpilibc', linkage: 'static'
                 lib project: ':wpimath', library: 'wpimath', linkage: 'static'
+                lib project: ':cameraserver', library: 'cameraserver', linkage: 'static'
                 lib project: ':ntcore', library: 'ntcore', linkage: 'static'
                 lib project: ':cscore', library: 'cscore', linkage: 'static'
                 project(':hal').addHalDependency(binary, 'static')
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
-                lib project: ':cameraserver', library: 'cameraserver', linkage: 'static'
                 if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
                     nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
                 }

shared/config.gradle

@@ -13,7 +13,7 @@ nativeUtils {
             niLibVersion = "2022.2.3"
             opencvVersion = "4.5.2-1"
             googleTestVersion = "1.9.0-5-437e100-1"
-            imguiVersion = "1.85-2"
+            imguiVersion = "1.86-1"
             wpimathVersion = "-1"
         }
     }

styleguide/spotbugs-exclude.xml

@@ -80,6 +80,14 @@
     <Bug pattern="UC_USELESS_VOID_METHOD" />
     <Class name="edu.wpi.first.wpilibj.templates.timed.Robot" />
   </Match>
+  <Match>
+    <Bug pattern="URF_UNREAD_FIELD" />
+    <Class name="edu.wpi.first.wpilibj.ADIS16448_IMU" />
+  </Match>
+  <Match>
+    <Bug pattern="URF_UNREAD_FIELD" />
+    <Class name="edu.wpi.first.wpilibj.ADIS16470_IMU" />
+  </Match>
   <Match>
     <Bug pattern="URF_UNREAD_PUBLIC_OR_PROTECTED_FIELD" />
   </Match>

wpigui/.styleguide

@@ -27,5 +27,4 @@ includeOtherLibs {
   ^imgui
   ^implot
   ^stb
-  ^wpi/
 }

wpigui/CMakeLists.txt

@@ -15,7 +15,7 @@ set_property(TARGET wpigui PROPERTY POSITION_INDEPENDENT_CODE ON)
 set_property(TARGET wpigui PROPERTY FOLDER "libraries")
 
 wpilib_target_warnings(wpigui)
-target_link_libraries(wpigui PUBLIC imgui wpiutil)
+target_link_libraries(wpigui PUBLIC imgui)
 
 target_include_directories(wpigui PUBLIC
                             $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>

wpigui/build.gradle

@@ -62,7 +62,6 @@ if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxra
                 }
                 binaries.all {
                     nativeUtils.useRequiredLibrary(it, 'imgui_static')
-                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                     if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
                         it.buildable = false
                         return
@@ -120,7 +119,6 @@ if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxra
                 binaries.all {
                     lib library: 'wpigui'
                     nativeUtils.useRequiredLibrary(it, 'imgui_static')
-                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                 }
             }
         }

wpigui/src/main/native/cpp/wpigui.cpp

@@ -15,7 +15,6 @@
 #include <imgui_internal.h>
 #include <implot.h>
 #include <stb_image.h>
-#include <wpi/fs.h>
 
 #include "wpigui_internal.h"
 
@@ -320,7 +319,7 @@ void gui::Main() {
 
   // Delete the save file if requested.
   if (!gContext->saveSettings && gContext->resetOnExit) {
-    fs::remove(fs::path{gContext->iniPath});
+    std::remove(gContext->iniPath.c_str());
   }
 
   glfwDestroyWindow(gContext->window);

wpilibNewCommands/build.gradle

@@ -75,6 +75,7 @@ model {
 
             if (it.component.name == "${nativeName}Dev") {
                 lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
+                lib project: ':wpiutil', library: 'wpiutilJNIShared', linkage: 'shared'
                 project(':hal').addHalJniDependency(it)
             }
 

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java

@@ -359,7 +359,7 @@ public class MecanumControllerCommand extends CommandBase {
         m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
     var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
 
-    targetWheelSpeeds.normalize(m_maxWheelVelocityMetersPerSecond);
+    targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
 
     var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
     var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java

@@ -6,7 +6,7 @@ package edu.wpi.first.wpilibj2.command.button;
 
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
-import edu.wpi.first.wpilibj.Debouncer;
+import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -24,7 +24,7 @@ import java.util.function.BooleanSupplier;
  * certain sensor input). For this, they only have to write the {@link Trigger#get()} method to get
  * the full functionality of the Trigger class.
  */
-public class Trigger {
+public class Trigger implements BooleanSupplier {
   private final BooleanSupplier m_isActive;
 
   /**
@@ -49,9 +49,25 @@ public class Trigger {
    *
    * <p>This method will be called repeatedly a command is linked to the Trigger.
    *
+   * <p>Functionally identical to {@link Trigger#getAsBoolean()}.
+   *
    * @return whether or not the trigger condition is active.
    */
   public boolean get() {
+    return this.getAsBoolean();
+  }
+
+  /**
+   * Returns whether or not the trigger is active.
+   *
+   * <p>This method will be called repeatedly a command is linked to the Trigger.
+   *
+   * <p>Functionally identical to {@link Trigger#get()}.
+   *
+   * @return whether or not the trigger condition is active.
+   */
+  @Override
+  public boolean getAsBoolean() {
     return m_isActive.getAsBoolean();
   }
 
@@ -365,13 +381,25 @@ public class Trigger {
    * Creates a new debounced trigger from this trigger - it will become active when this trigger has
    * been active for longer than the specified period.
    *
-   * @param seconds the debounce period
-   * @return the debounced trigger
+   * @param seconds The debounce period.
+   * @return The debounced trigger (rising edges debounced only)
    */
   public Trigger debounce(double seconds) {
+    return debounce(seconds, Debouncer.DebounceType.kRising);
+  }
+
+  /**
+   * Creates a new debounced trigger from this trigger - it will become active when this trigger has
+   * been active for longer than the specified period.
+   *
+   * @param seconds The debounce period.
+   * @param type The debounce type.
+   * @return The debounced trigger.
+   */
+  public Trigger debounce(double seconds, Debouncer.DebounceType type) {
     return new Trigger(
         new BooleanSupplier() {
-          Debouncer m_debouncer = new Debouncer(seconds);
+          Debouncer m_debouncer = new Debouncer(seconds, type);
 
           @Override
           public boolean getAsBoolean() {

wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp

@@ -278,7 +278,7 @@ void MecanumControllerCommand::Execute() {
       m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation());
   auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
 
-  targetWheelSpeeds.Normalize(m_maxWheelVelocity);
+  targetWheelSpeeds.Desaturate(m_maxWheelVelocity);
 
   auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
   auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp

@@ -4,7 +4,7 @@
 
 #include "frc2/command/button/Trigger.h"
 
-#include <frc/Debouncer.h>
+#include <frc/filter/Debouncer.h>
 
 #include "frc2/command/InstantCommand.h"
 
@@ -139,8 +139,10 @@ Trigger Trigger::CancelWhenActive(Command* command) {
   return *this;
 }
 
-Trigger Trigger::Debounce(units::second_t debounceTime) {
-  return Trigger([debouncer = frc::Debouncer(debounceTime), *this]() mutable {
-    return debouncer.Calculate(m_isActive());
-  });
+Trigger Trigger::Debounce(units::second_t debounceTime,
+                          frc::Debouncer::DebounceType type) {
+  return Trigger(
+      [debouncer = frc::Debouncer(debounceTime, type), *this]() mutable {
+        return debouncer.Calculate(m_isActive());
+      });
 }

wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h

@@ -9,6 +9,7 @@
 #include <memory>
 #include <utility>
 
+#include <frc/filter/Debouncer.h>
 #include <units/time.h>
 #include <wpi/span.h>
 
@@ -350,10 +351,13 @@ class Trigger {
    * Creates a new debounced trigger from this trigger - it will become active
    * when this trigger has been active for longer than the specified period.
    *
-   * @param debounceTime the debounce period
-   * @return the debounced trigger
+   * @param debounceTime The debounce period.
+   * @param type The debounce type.
+   * @return The debounced trigger.
    */
-  Trigger Debounce(units::second_t debounceTime);
+  Trigger Debounce(units::second_t debounceTime,
+                   frc::Debouncer::DebounceType type =
+                       frc::Debouncer::DebounceType::kRising);
 
  private:
   std::function<bool()> m_isActive;

wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp

@@ -0,0 +1,868 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 <iostream>
+#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_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);
+    }
+    std::cout << "Paused the IMU processing thread successfully!" << std::endl;
+    // 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);
+      }
+      std::cout << "Paused the auto SPI successfully!" << std::endl;
+    }
+  }
+  // There doesn't seem to be a SPI port active. Let's try to set one up
+  if (m_spi == nullptr) {
+    std::cout << "Setting up a new SPI port." << std::endl;
+    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 offset_data[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);
+    std::cout << "New IMU Processing thread activated!" << std::endl;
+  } else {
+    m_first_run = true;
+    m_thread_active = true;
+    std::cout << "Old IMU Processing thread re-activated!" << std::endl;
+  }
+  // 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 m_gyro_accum_x = 0.0;
+  double m_gyro_accum_y = 0.0;
+  double m_gyro_accum_z = 0.0;
+  for (int i = 0; i < gyroAverageSize; i++) {
+    m_gyro_accum_x += m_offset_buffer[i].m_accum_gyro_x;
+    m_gyro_accum_y += m_offset_buffer[i].m_accum_gyro_y;
+    m_gyro_accum_z += m_offset_buffer[i].m_accum_gyro_z;
+  }
+  m_gyro_offset_x = m_gyro_accum_x / gyroAverageSize;
+  m_gyro_offset_y = m_gyro_accum_y / gyroAverageSize;
+  m_gyro_offset_z = m_gyro_accum_z / gyroAverageSize;
+  m_integ_gyro_x = 0.0;
+  m_integ_gyro_y = 0.0;
+  m_integ_gyro_z = 0.0;
+  // std::cout << "Avg Size: " << gyroAverageSize << " X off: " <<
+  // m_gyro_offset_x << " Y off: " << m_gyro_offset_y << " Z off: " <<
+  // m_gyro_offset_z << std::endl;
+}
+
+/**
+ * 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_x = 0.0;
+  m_integ_gyro_y = 0.0;
+  m_integ_gyro_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;
+  std::cout << "Finished cleaning up after the IMU driver." << std::endl;
+}
+
+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;
+
+  // struct to store accumulate data
+  offset_data sample_data;
+
+  // 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_x = 0.0;
+  double gyro_y = 0.0;
+  double gyro_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_x_si = 0.0;
+  double gyro_y_si = 0.0;
+  // double gyro_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_x = BuffToShort(&buffer[i + 5]) * 0.04;
+          gyro_y = BuffToShort(&buffer[i + 7]) * 0.04;
+          gyro_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;
+
+          /*std::cout << BuffToShort(&buffer[i + 3]) << "," <<
+          BuffToShort(&buffer[i + 5]) << "," << BuffToShort(&buffer[i + 7]) <<
+          "," << BuffToShort(&buffer[i + 9]) << "," << BuffToShort(&buffer[i +
+          11]) << "," << BuffToShort(&buffer[i + 13]) << "," <<
+          BuffToShort(&buffer[i + 15]) << "," << BuffToShort(&buffer[i + 17]) <<
+          "," << BuffToShort(&buffer[i + 19]) << "," << BuffToShort(&buffer[i +
+          21]) << "," << BuffToShort(&buffer[i + 23]) << "," <<
+          BuffToShort(&buffer[i + 25]) << "," <<
+          BuffToShort(&buffer[i + 27]) << std::endl; */
+
+          // Convert scaled sensor data to SI units
+          gyro_x_si = gyro_x * deg_to_rad;
+          gyro_y_si = gyro_y * deg_to_rad;
+          // gyro_z_si = gyro_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_y_si);
+            compAngleY = CompFilterProcess(compAngleY, accelAngleY, -gyro_x_si);
+          }
+
+          // Update global variables and state
+          {
+            std::scoped_lock sync(m_mutex);
+            // Ignore first, integrated sample
+            if (m_first_run) {
+              m_integ_gyro_x = 0.0;
+              m_integ_gyro_y = 0.0;
+              m_integ_gyro_z = 0.0;
+            } else {
+              // Accumulate gyro for offset calibration
+              // Build most recent sample data
+              sample_data.m_accum_gyro_x = gyro_x;
+              sample_data.m_accum_gyro_y = gyro_y;
+              sample_data.m_accum_gyro_z = gyro_z;
+              // Add to buffer
+              bufferAvgIndex = m_accum_count % m_avg_size;
+              m_offset_buffer[bufferAvgIndex] = sample_data;
+              // 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_x = gyro_x;
+              m_gyro_y = gyro_y;
+              m_gyro_z = gyro_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_x += (gyro_x - m_gyro_offset_x) * m_dt;
+              m_integ_gyro_y += (gyro_y - m_gyro_offset_y) * m_dt;
+              m_integ_gyro_z += (gyro_z - m_gyro_offset_z) * m_dt;
+            }
+          }
+          m_first_run = false;
+        } else {
+          /*
+          // Print notification when crc fails and bad data is rejected
+          std::cout << "IMU Data CRC Mismatch Detected." << std::endl;
+          std::cout << "Calculated CRC: " << calc_crc << std::endl;
+          std::cout << "Read CRC: " << imu_crc << std::endl;
+          // DEBUG: Plot sub-array data in terminal
+          std::cout << BuffToUShort(&buffer[i + 3]) << "," <<
+          BuffToUShort(&buffer[i + 5]) << "," << BuffToUShort(&buffer[i + 7]) <<
+          "," << BuffToUShort(&buffer[i + 9]) << "," << BuffToUShort(&buffer[i +
+          11]) << "," << BuffToUShort(&buffer[i + 13]) << "," <<
+          BuffToUShort(&buffer[i + 15]) << "," << BuffToUShort(&buffer[i + 17])
+          << "," << BuffToUShort(&buffer[i + 19]) << "," <<
+          BuffToUShort(&buffer[i + 21]) << "," << BuffToUShort(&buffer[i + 23])
+          << "," << BuffToUShort(&buffer[i + 25]) << "," <<
+          BuffToUShort(&buffer[i + 27]) << std::endl; */
+        }
+      }
+    } else {
+      m_thread_idle = true;
+      data_count = 0;
+      data_remainder = 0;
+      data_to_read = 0;
+      previous_timestamp = 0.0;
+      gyro_x = 0.0;
+      gyro_y = 0.0;
+      gyro_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_x_si = 0.0;
+      gyro_y_si = 0.0;
+      // gyro_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::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_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_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_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());
+  });
+}

wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp

@@ -0,0 +1,816 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 <iostream>
+#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_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);
+    }
+    std::cout << "Paused the IMU processing thread successfully!" << std::endl;
+    // 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);
+      }
+      std::cout << "Paused the auto SPI successfully!" << std::endl;
+    }
+  }
+  // There doesn't seem to be a SPI port active. Let's try to set one up
+  if (m_spi == nullptr) {
+    std::cout << "Setting up a new SPI port." << std::endl;
+    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);
+    std::cout << "New IMU Processing thread activated!" << std::endl;
+  } else {
+    m_first_run = true;
+    m_thread_active = true;
+    std::cout << "Old IMU Processing thread re-activated!" << std::endl;
+  }
+  // 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;
+  }
+  std::cout << "Finished cleaning up after the IMU driver." << std::endl;
+}
+
+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_x = 0.0;
+  double gyro_y = 0.0;
+  double gyro_z = 0.0;
+  double accel_x = 0.0;
+  double accel_y = 0.0;
+  double accel_z = 0.0;
+  double gyro_x_si = 0.0;
+  double gyro_y_si = 0.0;
+  // double gyro_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)
+
+      /*
+      // DEBUG: Print buffer size and contents to terminal
+      std::cout << "Start - " << data_count << "," << data_remainder << "," <<
+      data_to_read << std::endl; for (int m = 0; m < data_to_read - 1; m++ )
+      {
+        std::cout << buffer[m] << ",";
+      }
+      std::cout << " " << std::endl;
+      std::cout << "End" << std::endl;
+      std::cout << "Reading " << data_count << " bytes." << std::endl;
+      */
+
+      // 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_x = (BuffToShort(&buffer[i + 7]) / 10.0);
+        gyro_y = (BuffToShort(&buffer[i + 9]) / 10.0);
+        gyro_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_x_si = gyro_x * deg_to_rad;
+        gyro_y_si = gyro_y * deg_to_rad;
+        // gyro_z_si = gyro_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];
+
+        /*
+        // DEBUG: Print timestamp and delta values
+        std::cout << previous_timestamp << "," << delta_x << "," << delta_y <<
+        "," << delta_z << std::endl;
+        */
+
+        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_y_si);
+          compAngleY = CompFilterProcess(compAngleY, accelAngleY, gyro_x_si);
+        }
+
+        // DEBUG: Print accumulated values
+        // std::cout << m_compAngleX << "," << m_compAngleY << std::endl;
+
+        {
+          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_x = gyro_x;
+          m_gyro_y = gyro_y;
+          m_gyro_z = gyro_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_x = 0.0;
+      gyro_y = 0.0;
+      gyro_z = 0.0;
+      accel_x = 0.0;
+      accel_y = 0.0;
+      accel_z = 0.0;
+      gyro_x_si = 0.0;
+      gyro_y_si = 0.0;
+      // gyro_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;
+}
+
+/**
+ * @brief Returns the current integrated angle for the axis specified.
+ *
+ * @param m_yaw_axis An enum indicating the axis chosen to act as the yaw axis.
+ *
+ * @return The current integrated angle in degrees.
+ *
+ * This function returns the most recent integrated angle for the axis chosen by
+ *m_yaw_axis. This function is most useful in situations where the yaw axis may
+ *not coincide with the IMU Z axis.
+ **/
+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::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());
+  });
+}

wpilibc/src/main/native/cpp/Compressor.cpp

@@ -56,6 +56,10 @@ units::volt_t Compressor::GetAnalogVoltage() const {
   return m_module->GetAnalogVoltage(0);
 }
 
+units::pounds_per_square_inch_t Compressor::GetPressure() const {
+  return m_module->GetPressure(0);
+}
+
 void Compressor::Disable() {
   m_module->DisableCompressor();
 }
@@ -64,14 +68,14 @@ void Compressor::EnableDigital() {
   m_module->EnableCompressorDigital();
 }
 
-void Compressor::EnableAnalog(units::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) {
-  m_module->EnableCompressorAnalog(minAnalogVoltage, maxAnalogVoltage);
+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::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) {
-  m_module->EnableCompressorHybrid(minAnalogVoltage, maxAnalogVoltage);
+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 {

wpilibc/src/main/native/cpp/I2C.cpp

@@ -16,6 +16,14 @@ using namespace frc;
 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);
 

wpilibc/src/main/native/cpp/PneumaticHub.cpp

@@ -13,9 +13,24 @@
 #include "frc/Errors.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 +54,15 @@ class PneumaticHub::DataStore {
     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 && 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); }
@@ -93,17 +117,51 @@ void PneumaticHub::EnableCompressorDigital() {
   FRC_ReportError(status, "Module {}", m_module);
 }
 
-void PneumaticHub::EnableCompressorAnalog(units::volt_t minAnalogVoltage,
-                                          units::volt_t maxAnalogVoltage) {
+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::volt_t minAnalogVoltage,
-                                          units::volt_t maxAnalogVoltage) {
+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);
@@ -286,6 +344,15 @@ units::volt_t PneumaticHub::GetAnalogVoltage(int channel) const {
   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};
 }

wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp

@@ -97,14 +97,16 @@ void PneumaticsControlModule::EnableCompressorDigital() {
 }
 
 void PneumaticsControlModule::EnableCompressorAnalog(
-    units::volt_t minAnalogVoltage, units::volt_t maxAnalogVoltage) {
+    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_CheckErrorStatus(status, "Module {}", m_module);
 }
 
 void PneumaticsControlModule::EnableCompressorHybrid(
-    units::volt_t minAnalogVoltage, units::volt_t maxAnalogVoltage) {
+    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_CheckErrorStatus(status, "Module {}", m_module);
@@ -265,6 +267,11 @@ 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};
 }

wpilibc/src/main/native/cpp/counter/Tachometer.cpp

@@ -64,17 +64,21 @@ void Tachometer::SetEdgesPerRevolution(int edges) {
   m_edgesPerRevolution = edges;
 }
 
-units::revolutions_per_minute_t Tachometer::GetRevolutionsPerMinute() const {
+units::turns_per_second_t Tachometer::GetRevolutionsPerSecond() const {
   auto period = GetPeriod();
   if (period.to<double>() == 0) {
-    return units::revolutions_per_minute_t{0.0};
+    return units::turns_per_second_t{0.0};
   }
   int edgesPerRevolution = GetEdgesPerRevolution();
   if (edgesPerRevolution == 0) {
-    return units::revolutions_per_minute_t{0.0};
+    return units::turns_per_second_t{0.0};
   }
   auto rotationHz = ((1.0 / edgesPerRevolution) / period);
-  return units::revolutions_per_minute_t{rotationHz.to<double>() * 60};
+  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 {
@@ -111,6 +115,8 @@ void Tachometer::SetUpdateWhenEmpty(bool updateWhenEmpty) {
 
 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);
 }

wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp

@@ -102,7 +102,7 @@ KilloughDrive::WheelSpeeds KilloughDrive::DriveCartesianIK(double ySpeed,
   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]};
 }

wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp

@@ -103,7 +103,7 @@ MecanumDrive::WheelSpeeds MecanumDrive::DriveCartesianIK(double ySpeed,
   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]};

wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp

@@ -35,7 +35,7 @@ double RobotDriveBase::ApplyDeadband(double value, double deadband) {
   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]);

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

wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#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_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::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());
+}

wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#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_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::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());
+}

wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp

@@ -43,8 +43,8 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim(
 
 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,

wpilibc/src/main/native/include/frc/ADIS16448_IMU.h

@@ -0,0 +1,405 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*----------------------------------------------------------------------------*/
+/* 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 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 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 offset_data {
+    double m_accum_gyro_x = 0.0;
+    double m_accum_gyro_y = 0.0;
+    double m_accum_gyro_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_x = 0.0;
+  double m_gyro_y = 0.0;
+  double m_gyro_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
+  offset_data* m_offset_buffer = nullptr;
+
+  double m_gyro_offset_x = 0.0;
+  double m_gyro_offset_y = 0.0;
+  double m_gyro_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_x = 0.0;
+  double m_integ_gyro_y = 0.0;
+  double m_integ_gyro_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;
+  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_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

wpilibc/src/main/native/include/frc/ADIS16470_IMU.h

@@ -0,0 +1,404 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 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_x, m_gyro_y, m_gyro_z, m_accel_x, m_accel_y, 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;
+  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_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

wpilibc/src/main/native/include/frc/Compressor.h

@@ -104,6 +104,14 @@ class Compressor : public wpi::Sendable,
    */
   units::volt_t GetAnalogVoltage() const;
 
+  /**
+   * Query the analog sensor pressure (on channel 0) (if supported). Note this
+   * is only for use with the REV Analog Pressure Sensor.
+   *
+   * @return The analog sensor pressure, in PSI
+   */
+  units::pounds_per_square_inch_t GetPressure() const;
+
   /**
    * Disable the compressor.
    */
@@ -115,26 +123,28 @@ class Compressor : public wpi::Sendable,
   void EnableDigital();
 
   /**
-   * Enable compressor closed loop control using analog input.
+   * 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 minAnalogVoltage The minimum voltage to enable compressor
-   * @param maxAnalogVoltage The maximum voltage to disable compressor
+   * @param minPressure The minimum pressure in PSI to enable compressor
+   * @param maxPressure The maximum pressure in PSI to disable compressor
    */
-  void EnableAnalog(units::volt_t minAnalogVoltage,
-                    units::volt_t maxAnalogVoltage);
+  void EnableAnalog(units::pounds_per_square_inch_t minPressure,
+                    units::pounds_per_square_inch_t maxPressure);
 
   /**
-   * Enable compressor closed loop control using hybrid input.
+   * 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 minAnalogVoltage The minimum voltage to enable compressor
-   * @param maxAnalogVoltage The maximum voltage to disable compressor
+   * @param minPressure The minimum pressure in PSI to enable compressor
+   * @param maxPressure The maximum pressure in PSI to disable compressor
    */
-  void EnableHybrid(units::volt_t minAnalogVoltage,
-                    units::volt_t maxAnalogVoltage);
+  void EnableHybrid(units::pounds_per_square_inch_t minPressure,
+                    units::pounds_per_square_inch_t maxPressure);
 
   CompressorConfigType GetConfigType() const;
 

wpilibc/src/main/native/include/frc/PneumaticHub.h

@@ -26,11 +26,13 @@ class PneumaticHub : public PneumaticsBase {
 
   void EnableCompressorDigital() override;
 
-  void EnableCompressorAnalog(units::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) override;
+  void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
 
-  void EnableCompressorHybrid(units::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) override;
+  void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
 
   CompressorConfigType GetCompressorConfigType() const override;
 
@@ -127,6 +129,8 @@ class PneumaticHub : public PneumaticsBase {
 
   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;

wpilibc/src/main/native/include/frc/PneumaticsBase.h

@@ -7,6 +7,7 @@
 #include <memory>
 
 #include <units/current.h>
+#include <units/pressure.h>
 #include <units/time.h>
 #include <units/voltage.h>
 
@@ -31,11 +32,13 @@ class PneumaticsBase {
 
   virtual void EnableCompressorDigital() = 0;
 
-  virtual void EnableCompressorAnalog(units::volt_t minAnalogVoltage,
-                                      units::volt_t maxAnalogVoltage) = 0;
+  virtual void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) = 0;
 
-  virtual void EnableCompressorHybrid(units::volt_t minAnalogVoltage,
-                                      units::volt_t maxAnalogVoltage) = 0;
+  virtual void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) = 0;
 
   virtual CompressorConfigType GetCompressorConfigType() const = 0;
 
@@ -63,6 +66,8 @@ class PneumaticsBase {
 
   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;

wpilibc/src/main/native/include/frc/PneumaticsControlModule.h

@@ -26,11 +26,13 @@ class PneumaticsControlModule : public PneumaticsBase {
 
   void EnableCompressorDigital() override;
 
-  void EnableCompressorAnalog(units::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) override;
+  void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
 
-  void EnableCompressorHybrid(units::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) override;
+  void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
 
   CompressorConfigType GetCompressorConfigType() const override;
 
@@ -74,6 +76,8 @@ class PneumaticsControlModule : public PneumaticsBase {
 
   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;

wpilibc/src/main/native/include/frc/Servo.h

@@ -46,7 +46,8 @@ class Servo : public PWM {
    * 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.
    */
@@ -71,8 +72,8 @@ class Servo : public PWM {
   /**
    * 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.
    */

wpilibc/src/main/native/include/frc/counter/Tachometer.h

@@ -75,6 +75,15 @@ class Tachometer : public wpi::Sendable,
    */
   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.
    *

wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h

@@ -82,10 +82,10 @@ class RobotDriveBase : public MotorSafety {
   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;

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 @@ using CS_Source = CS_Handle;  // NOLINT
 
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
+#include <wpi/span.h>
 
 namespace frc {
 
@@ -28,8 +33,8 @@ class SendableCameraWrapper;
 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 @@ class SendableCameraWrapper
    * 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 @@ class SendableCameraWrapper
   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 @@ class SendableCameraWrapper
 };
 
 #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(
 }
 
 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

wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h

@@ -127,6 +127,18 @@ class ShuffleboardContainer : public virtual ShuffleboardValue {
    */
   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.
    *
@@ -526,4 +538,10 @@ inline frc::ComplexWidget& frc::ShuffleboardContainer::Add(
     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

wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h

@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <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 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_simAccelX;
+  hal::SimDouble m_simAccelY;
+  hal::SimDouble m_simAccelZ;
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h

@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <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 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_simAccelX;
+  hal::SimDouble m_simAccelY;
+  hal::SimDouble m_simAccelZ;
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h

@@ -45,6 +45,8 @@ class LinearSystemSim {
     m_u = Eigen::Vector<double, Inputs>::Zero();
   }
 
+  virtual ~LinearSystemSim() = default;
+
   /**
    * Updates the simulation.
    *
@@ -136,7 +138,7 @@ class LinearSystemSim {
    * @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());
   }
 

wpilibc/src/test/native/cpp/DebouncerTest.cpp

@@ -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));
-}

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 @@ TEST(DifferentialDriveSimTest, Convergence) {
   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 @@ TEST(DifferentialDriveSimTest, Convergence) {
   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 @@ TEST(DifferentialDriveSimTest, Current) {
   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);

wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp

@@ -129,7 +129,7 @@ TEST(REVPHSimTest, SetEnableAnalog) {
   ph.DisableCompressor();
   EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Disabled);
 
-  ph.EnableCompressorAnalog(1_V, 2_V);
+  ph.EnableCompressorAnalog(1_psi, 2_psi);
   EXPECT_EQ(sim.GetCompressorConfigType(),
             static_cast<int>(CompressorConfigType::Analog));
   EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Analog);
@@ -150,7 +150,7 @@ TEST(REVPHSimTest, SetEnableHybrid) {
   ph.DisableCompressor();
   EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Disabled);
 
-  ph.EnableCompressorHybrid(1_V, 2_V);
+  ph.EnableCompressorHybrid(1_psi, 2_psi);
   EXPECT_EQ(sim.GetCompressorConfigType(),
             static_cast<int>(CompressorConfigType::Hybrid));
   EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Hybrid);

wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp

@@ -18,9 +18,16 @@ class Robot : public frc::TimedRobot {
   frc::Joystick m_stick{0};
 
  public:
+  void RobotInit() override {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotor.SetInverted(true);
+  }
+
   void TeleopPeriodic() override {
     // Drive with arcade style
-    m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
+    m_robotDrive.ArcadeDrive(-m_stick.GetY(), m_stick.GetX());
   }
 };
 

wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp

@@ -18,11 +18,18 @@ class Robot : public frc::TimedRobot {
   frc::XboxController m_driverController{0};
 
  public:
+  void RobotInit() override {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotor.SetInverted(true);
+  }
+
   void TeleopPeriodic() override {
     // Drive with split arcade style
     // That means that the Y axis of the left stick moves forward
     // and backward, and the X of the right stick turns left and right.
-    m_robotDrive.ArcadeDrive(m_driverController.GetLeftY(),
+    m_robotDrive.ArcadeDrive(-m_driverController.GetLeftY(),
                              m_driverController.GetRightX());
   }
 };

wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp

@@ -17,7 +17,7 @@ RobotContainer::RobotContainer() {
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+        m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
                             m_driverController.GetRightX());
       },
       {&m_drive}));

wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp

@@ -16,6 +16,10 @@ DriveSubsystem::DriveSubsystem()
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotors.SetInverted(true);
 }
 
 void DriveSubsystem::Periodic() {

wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp

@@ -17,7 +17,7 @@ RobotContainer::RobotContainer() {
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+        m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
                             m_driverController.GetRightX());
       },
       {&m_drive}));

wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp

@@ -16,6 +16,10 @@ DriveSubsystem::DriveSubsystem()
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotors.SetInverted(true);
 }
 
 void DriveSubsystem::Periodic() {

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp

@@ -18,7 +18,7 @@ RobotContainer::RobotContainer() {
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+        m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
                             m_driverController.GetRightX());
       },
       {&m_drive}));

wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp

@@ -16,7 +16,7 @@ RobotContainer::RobotContainer() {
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+        m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
                             m_driverController.GetRightX());
       },
       {&m_drive}));

wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp

@@ -11,7 +11,11 @@
 class Robot : public frc::TimedRobot {
  public:
   Robot() {
+    m_right.SetInverted(true);
     m_robotDrive.SetExpiration(100_ms);
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
     m_timer.Start();
   }
 
@@ -35,7 +39,7 @@ class Robot : public frc::TimedRobot {
 
   void TeleopPeriodic() override {
     // Drive with arcade style (use right stick)
-    m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
+    m_robotDrive.ArcadeDrive(-m_stick.GetY(), m_stick.GetX());
   }
 
   void TestInit() override {}

wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp

@@ -17,7 +17,13 @@
  */
 class Robot : public frc::TimedRobot {
  public:
-  void RobotInit() override { m_gyro.SetSensitivity(kVoltsPerDegreePerSecond); }
+  void RobotInit() override {
+    m_gyro.SetSensitivity(kVoltsPerDegreePerSecond);
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_right.SetInverted(true);
+  }
 
   /**
    * The motor speed is set from the joystick while the DifferentialDrive
@@ -28,7 +34,7 @@ class Robot : public frc::TimedRobot {
     double turningValue = (kAngleSetpoint - m_gyro.GetAngle()) * kP;
     // Invert the direction of the turn if we are going backwards
     turningValue = std::copysign(turningValue, m_joystick.GetY());
-    m_robotDrive.ArcadeDrive(m_joystick.GetY(), turningValue);
+    m_robotDrive.ArcadeDrive(-m_joystick.GetY(), turningValue);
   }
 
  private:

wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp

@@ -19,7 +19,7 @@ RobotContainer::RobotContainer() {
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+        m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
                             m_driverController.GetRightX());
       },
       {&m_drive}));

wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp

@@ -16,10 +16,10 @@
 class Robot : public frc::TimedRobot {
  public:
   void RobotInit() override {
-    // Invert the left side motors. You may need to change or remove this to
+    // Invert the right side motors. You may need to change or remove this to
     // match your robot.
-    m_frontLeft.SetInverted(true);
-    m_rearLeft.SetInverted(true);
+    m_frontRight.SetInverted(true);
+    m_rearRight.SetInverted(true);
 
     m_gyro.SetSensitivity(kVoltsPerDegreePerSecond);
   }

wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp

@@ -23,7 +23,7 @@ RobotContainer::RobotContainer() {
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+        m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
                             m_driverController.GetRightX());
       },
       {&m_drive}));

wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp

@@ -27,7 +27,7 @@ RobotContainer::RobotContainer() {
 
   // Set up default drive command
   m_drive.SetDefaultCommand(DefaultDrive(
-      &m_drive, [this] { return m_driverController.GetLeftY(); },
+      &m_drive, [this] { return -m_driverController.GetLeftY(); },
       [this] { return m_driverController.GetRightX(); }));
 }
 

wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp

@@ -47,7 +47,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
       fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
                           xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
                     : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
-  wheelSpeeds.Normalize(kMaxSpeed);
+  wheelSpeeds.Desaturate(kMaxSpeed);
   SetSpeeds(wheelSpeeds);
 }
 

wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h

@@ -20,7 +20,14 @@
  */
 class Drivetrain {
  public:
-  Drivetrain() { m_gyro.Reset(); }
+  Drivetrain() {
+    m_gyro.Reset();
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_frontRightMotor.SetInverted(true);
+    m_backRightMotor.SetInverted(true);
+  }
 
   frc::MecanumDriveWheelSpeeds GetCurrentState() const;
   void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);

wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp

@@ -34,6 +34,11 @@ DriveSubsystem::DriveSubsystem()
   m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_frontRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rearRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_frontRight.SetInverted(true);
+  m_rearRight.SetInverted(true);
 }
 
 void DriveSubsystem::Periodic() {

wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp

@@ -14,10 +14,10 @@
 class Robot : public frc::TimedRobot {
  public:
   void RobotInit() override {
-    // Invert the left side motors. You may need to change or remove this to
+    // Invert the right side motors. You may need to change or remove this to
     // match your robot.
-    m_frontLeft.SetInverted(true);
-    m_rearLeft.SetInverted(true);
+    m_frontRight.SetInverted(true);
+    m_rearRight.SetInverted(true);
   }
 
   void TeleopPeriodic() override {

wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp

@@ -44,7 +44,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
       fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
                           xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
                     : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
-  wheelSpeeds.Normalize(kMaxSpeed);
+  wheelSpeeds.Desaturate(kMaxSpeed);
   SetSpeeds(wheelSpeeds);
 }
 

wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h

@@ -21,7 +21,14 @@
  */
 class Drivetrain {
  public:
-  Drivetrain() { m_gyro.Reset(); }
+  Drivetrain() {
+    m_gyro.Reset();
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_frontRightMotor.SetInverted(true);
+    m_backRightMotor.SetInverted(true);
+  }
 
   frc::MecanumDriveWheelSpeeds GetCurrentState() const;
   void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp

@@ -42,7 +42,7 @@ void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
 
 void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
   m_leftMotors.SetVoltage(left);
-  m_rightMotors.SetVoltage(-right);
+  m_rightMotors.SetVoltage(right);
   m_drive.Feed();
 }
 

wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp

@@ -43,7 +43,7 @@ void Drivetrain::SimulationPeriodic() {
   // voltages make the right side move forward.
   m_drivetrainSimulator.SetInputs(units::volt_t{m_leftLeader.Get()} *
                                       frc::RobotController::GetInputVoltage(),
-                                  units::volt_t{-m_rightLeader.Get()} *
+                                  units::volt_t{m_rightLeader.Get()} *
                                       frc::RobotController::GetInputVoltage());
   m_drivetrainSimulator.Update(20_ms);
 

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp

@@ -28,7 +28,7 @@ RobotContainer::RobotContainer() {
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+        m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
                             m_driverController.GetRightX());
       },
       {&m_drive}));

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp

@@ -42,7 +42,7 @@ void DriveSubsystem::SimulationPeriodic() {
   // voltages make the right side move forward.
   m_drivetrainSimulator.SetInputs(units::volt_t{m_leftMotors.Get()} *
                                       frc::RobotController::GetInputVoltage(),
-                                  units::volt_t{-m_rightMotors.Get()} *
+                                  units::volt_t{m_rightMotors.Get()} *
                                       frc::RobotController::GetInputVoltage());
   m_drivetrainSimulator.Update(20_ms);
 
@@ -64,7 +64,7 @@ void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
 
 void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
   m_leftMotors.SetVoltage(left);
-  m_rightMotors.SetVoltage(-right);
+  m_rightMotors.SetVoltage(right);
   m_drive.Feed();
 }
 

wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp

@@ -12,7 +12,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
                           xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
                     : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
 
-  m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed);
+  m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
 
   auto [fl, fr, bl, br] = states;
 

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp

@@ -54,7 +54,7 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
                           xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
                     : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
 
-  kDriveKinematics.NormalizeWheelSpeeds(&states, AutoConstants::kMaxSpeed);
+  kDriveKinematics.DesaturateWheelSpeeds(&states, AutoConstants::kMaxSpeed);
 
   auto [fl, fr, bl, br] = states;
 
@@ -66,11 +66,11 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
 
 void DriveSubsystem::SetModuleStates(
     wpi::array<frc::SwerveModuleState, 4> desiredStates) {
-  kDriveKinematics.NormalizeWheelSpeeds(&desiredStates,
-                                        AutoConstants::kMaxSpeed);
+  kDriveKinematics.DesaturateWheelSpeeds(&desiredStates,
+                                         AutoConstants::kMaxSpeed);
   m_frontLeft.SetDesiredState(desiredStates[0]);
-  m_rearLeft.SetDesiredState(desiredStates[1]);
-  m_frontRight.SetDesiredState(desiredStates[2]);
+  m_frontRight.SetDesiredState(desiredStates[1]);
+  m_rearLeft.SetDesiredState(desiredStates[2]);
   m_rearRight.SetDesiredState(desiredStates[3]);
 }
 

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp

@@ -16,7 +16,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
                           xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
                     : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
 
-  m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed);
+  m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
 
   auto [fl, fr, bl, br] = states;
 

wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp

@@ -27,8 +27,8 @@ class Robot : public frc::TimedRobot {
 
   void TeleopPeriodic() override {
     // Drive with tank style
-    m_robotDrive.TankDrive(m_driverController.GetLeftY(),
-                           m_driverController.GetRightY());
+    m_robotDrive.TankDrive(-m_driverController.GetLeftY(),
+                           -m_driverController.GetRightY());
   }
 };
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java

@@ -0,0 +1,991 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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) FIRST 2016. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+// import java.lang.FdLibm.Pow;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+// CHECKSTYLE.OFF: TypeName
+// CHECKSTYLE.OFF: MemberName
+// CHECKSTYLE.OFF: SummaryJavadoc
+// CHECKSTYLE.OFF: UnnecessaryParentheses
+// CHECKSTYLE.OFF: OverloadMethodsDeclarationOrder
+// CHECKSTYLE.OFF: NonEmptyAtclauseDescription
+// CHECKSTYLE.OFF: MissingOverride
+// CHECKSTYLE.OFF: AtclauseOrder
+// CHECKSTYLE.OFF: LocalVariableName
+// CHECKSTYLE.OFF: RedundantModifier
+// CHECKSTYLE.OFF: AbbreviationAsWordInName
+// CHECKSTYLE.OFF: ParameterName
+// CHECKSTYLE.OFF: EmptyCatchBlock
+// CHECKSTYLE.OFF: MissingJavadocMethod
+// CHECKSTYLE.OFF: MissingSwitchDefault
+// CHECKSTYLE.OFF: VariableDeclarationUsageDistance
+// CHECKSTYLE.OFF: ArrayTypeStyle
+
+/** This class is for the ADIS16470 IMU that connects to the RoboRIO SPI port. */
+@SuppressWarnings({
+  "unused",
+  "PMD.RedundantFieldInitializer",
+  "PMD.ImmutableField",
+  "PMD.SingularField",
+  "PMD.CollapsibleIfStatements",
+  "PMD.MissingOverride",
+  "PMD.EmptyIfStmt",
+  "PMD.EmptyStatementNotInLoop"
+})
+public class ADIS16470_IMU implements AutoCloseable, NTSendable {
+  /* ADIS16470 Register Map Declaration */
+  private static final int FLASH_CNT = 0x00; // Flash memory write count
+  private static final int DIAG_STAT = 0x02; // Diagnostic and operational status
+  private static final int X_GYRO_LOW = 0x04; // X-axis gyroscope output, lower word
+  private static final int X_GYRO_OUT = 0x06; // X-axis gyroscope output, upper word
+  private static final int Y_GYRO_LOW = 0x08; // Y-axis gyroscope output, lower word
+  private static final int Y_GYRO_OUT = 0x0A; // Y-axis gyroscope output, upper word
+  private static final int Z_GYRO_LOW = 0x0C; // Z-axis gyroscope output, lower word
+  private static final int Z_GYRO_OUT = 0x0E; // Z-axis gyroscope output, upper word
+  private static final int X_ACCL_LOW = 0x10; // X-axis accelerometer output, lower word
+  private static final int X_ACCL_OUT = 0x12; // X-axis accelerometer output, upper word
+  private static final int Y_ACCL_LOW = 0x14; // Y-axis accelerometer output, lower word
+  private static final int Y_ACCL_OUT = 0x16; // Y-axis accelerometer output, upper word
+  private static final int Z_ACCL_LOW = 0x18; // Z-axis accelerometer output, lower word
+  private static final int Z_ACCL_OUT = 0x1A; // Z-axis accelerometer output, upper word
+  private static final int TEMP_OUT = 0x1C; // Temperature output (internal, not calibrated)
+  private static final int TIME_STAMP = 0x1E; // PPS mode time stamp
+  private static final int X_DELTANG_LOW = 0x24; // X-axis delta angle output, lower word
+  private static final int X_DELTANG_OUT = 0x26; // X-axis delta angle output, upper word
+  private static final int Y_DELTANG_LOW = 0x28; // Y-axis delta angle output, lower word
+  private static final int Y_DELTANG_OUT = 0x2A; // Y-axis delta angle output, upper word
+  private static final int Z_DELTANG_LOW = 0x2C; // Z-axis delta angle output, lower word
+  private static final int Z_DELTANG_OUT = 0x2E; // Z-axis delta angle output, upper word
+  private static final int X_DELTVEL_LOW = 0x30; // X-axis delta velocity output, lower word
+  private static final int X_DELTVEL_OUT = 0x32; // X-axis delta velocity output, upper word
+  private static final int Y_DELTVEL_LOW = 0x34; // Y-axis delta velocity output, lower word
+  private static final int Y_DELTVEL_OUT = 0x36; // Y-axis delta velocity output, upper word
+  private static final int Z_DELTVEL_LOW = 0x38; // Z-axis delta velocity output, lower word
+  private static final int Z_DELTVEL_OUT = 0x3A; // Z-axis delta velocity output, upper word
+  private static final int XG_BIAS_LOW =
+      0x40; // X-axis gyroscope bias offset correction, lower word
+  private static final int XG_BIAS_HIGH =
+      0x42; // X-axis gyroscope bias offset correction, upper word
+  private static final int YG_BIAS_LOW =
+      0x44; // Y-axis gyroscope bias offset correction, lower word
+  private static final int YG_BIAS_HIGH =
+      0x46; // Y-axis gyroscope bias offset correction, upper word
+  private static final int ZG_BIAS_LOW =
+      0x48; // Z-axis gyroscope bias offset correction, lower word
+  private static final int ZG_BIAS_HIGH =
+      0x4A; // Z-axis gyroscope bias offset correction, upper word
+  private static final int XA_BIAS_LOW =
+      0x4C; // X-axis accelerometer bias offset correction, lower word
+  private static final int XA_BIAS_HIGH =
+      0x4E; // X-axis accelerometer bias offset correction, upper word
+  private static final int YA_BIAS_LOW =
+      0x50; // Y-axis accelerometer bias offset correction, lower word
+  private static final int YA_BIAS_HIGH =
+      0x52; // Y-axis accelerometer bias offset correction, upper word
+  private static final int ZA_BIAS_LOW =
+      0x54; // Z-axis accelerometer bias offset correction, lower word
+  private static final int ZA_BIAS_HIGH =
+      0x56; // Z-axis accelerometer bias offset correction, upper word
+  private static final int FILT_CTRL = 0x5C; // Filter control
+  private static final int MSC_CTRL = 0x60; // Miscellaneous control
+  private static final int UP_SCALE = 0x62; // Clock scale factor, PPS mode
+  private static final int DEC_RATE = 0x64; // Decimation rate control (output data rate)
+  private static final int NULL_CNFG = 0x66; // Auto-null configuration control
+  private static final int GLOB_CMD = 0x68; // Global commands
+  private static final int FIRM_REV = 0x6C; // Firmware revision
+  private static final int FIRM_DM = 0x6E; // Firmware revision date, month and day
+  private static final int FIRM_Y = 0x70; // Firmware revision date, year
+  private static final int PROD_ID = 0x72; // Product identification
+  private static final int SERIAL_NUM = 0x74; // Serial number (relative to assembly lot)
+  private static final int USER_SCR1 = 0x76; // User scratch register 1
+  private static final int USER_SCR2 = 0x78; // User scratch register 2
+  private static final int USER_SCR3 = 0x7A; // User scratch register 3
+  private static final int FLSHCNT_LOW = 0x7C; // Flash update count, lower word
+  private static final int FLSHCNT_HIGH = 0x7E; // Flash update count, upper word
+
+  private static final byte[] m_autospi_x_packet = {
+    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
+  };
+
+  private static final byte[] m_autospi_y_packet = {
+    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
+  };
+
+  private static final byte[] m_autospi_z_packet = {
+    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
+  };
+
+  public enum 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);
+
+    private int value;
+
+    private CalibrationTime(int value) {
+      this.value = value;
+    }
+  }
+
+  public enum IMUAxis {
+    kX,
+    kY,
+    kZ
+  }
+
+  // Static Constants
+  private static final double delta_angle_sf = 2160.0 / 2147483648.0; /* 2160 / (2^31) */
+  private static final double rad_to_deg = 57.2957795;
+  private static final double deg_to_rad = 0.0174532;
+  private static final double grav = 9.81;
+
+  // User-specified yaw axis
+  private IMUAxis m_yaw_axis;
+
+  // Instant raw outputs
+  private double m_gyro_x = 0.0;
+  private double m_gyro_y = 0.0;
+  private double m_gyro_z = 0.0;
+  private double m_accel_x = 0.0;
+  private double m_accel_y = 0.0;
+  private double m_accel_z = 0.0;
+
+  // Integrated gyro angle
+  private double m_integ_angle = 0.0;
+
+  // Complementary filter variables
+  private double m_dt = 0.0;
+  private double m_alpha = 0.0;
+  private double m_tau = 1.0;
+  private double m_compAngleX = 0.0;
+  private double m_compAngleY = 0.0;
+  private double m_accelAngleX = 0.0;
+  private double m_accelAngleY = 0.0;
+
+  // State variables
+  private volatile boolean m_thread_active = false;
+  private int m_calibration_time = 0;
+  private volatile boolean m_first_run = true;
+  private volatile boolean m_thread_idle = false;
+  private boolean m_auto_configured = false;
+  private double m_scaled_sample_rate = 2500.0;
+
+  // Resources
+  private SPI m_spi;
+  private SPI.Port m_spi_port;
+  private DigitalInput m_auto_interrupt;
+  private DigitalOutput m_reset_out;
+  private DigitalInput m_reset_in;
+  private DigitalOutput m_status_led;
+  private Thread m_acquire_task;
+
+  private SimDevice m_simDevice;
+  private SimDouble m_simGyroAngleX;
+  private SimDouble m_simGyroAngleY;
+  private SimDouble m_simGyroAngleZ;
+  private SimDouble m_simAccelX;
+  private SimDouble m_simAccelY;
+  private SimDouble m_simAccelZ;
+
+  private static class AcquireTask implements Runnable {
+    private ADIS16470_IMU imu;
+
+    public AcquireTask(ADIS16470_IMU imu) {
+      this.imu = imu;
+    }
+
+    @Override
+    public void run() {
+      imu.acquire();
+    }
+  }
+
+  public ADIS16470_IMU() {
+    this(IMUAxis.kZ, SPI.Port.kOnboardCS0, CalibrationTime._4s);
+  }
+
+  /**
+   * @param yaw_axis The axis that measures the yaw
+   * @param port The SPI Port the gyro is plugged into
+   * @param cal_time Calibration time
+   */
+  public ADIS16470_IMU(IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) {
+    m_yaw_axis = yaw_axis;
+    m_calibration_time = cal_time.value;
+    m_spi_port = port;
+
+    m_acquire_task = new Thread(new AcquireTask(this));
+
+    m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value);
+    if (m_simDevice != null) {
+      m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0);
+      m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0);
+      m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0);
+      m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0);
+      m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0);
+      m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0);
+    }
+
+    if (m_simDevice == null) {
+      // 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.
+      m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
+      Timer.delay(0.01); // Wait 10ms
+      m_reset_out.close();
+      m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high
+      Timer.delay(0.25); // Wait 250ms for reset to complete
+
+      if (!switchToStandardSPI()) {
+        return;
+      }
+
+      // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) =
+      // 400Hz)
+      writeRegister(DEC_RATE, 4);
+
+      // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and
+      // PoP
+      writeRegister(MSC_CTRL, 1);
+
+      // Configure IMU internal Bartlett filter
+      writeRegister(FILT_CTRL, 0);
+
+      // Configure continuous bias calibration time based on user setting
+      writeRegister(NULL_CNFG, (m_calibration_time | 0x0700));
+
+      // Notify DS that IMU calibration delay is active
+      DriverStation.reportWarning(
+          "ADIS16470 IMU Detected. Starting initial calibration delay.", false);
+
+      // Wait for samples to accumulate internal to the IMU (110% of user-defined
+      // time)
+      try {
+        Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000));
+      } catch (InterruptedException e) {
+      }
+
+      // 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
+      DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false);
+
+      // Drive "Ready" LED low
+      m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low
+    }
+
+    // Report usage and post data to DS
+    HAL.report(tResourceType.kResourceType_ADIS16470, 0);
+  }
+
+  /**
+   * @param buf
+   * @return
+   */
+  private static int toUShort(ByteBuffer buf) {
+    return (buf.getShort(0)) & 0xFFFF;
+  }
+
+  /**
+   * @param sint
+   * @return
+   */
+  private static long toULong(int sint) {
+    return sint & 0x00000000FFFFFFFFL;
+  }
+
+  /**
+   * @param buf
+   * @return
+   */
+  private static int toShort(int... buf) {
+    return (short) (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0));
+  }
+
+  /**
+   * @param buf
+   * @return
+   */
+  private static int toInt(int... buf) {
+    return (int)
+        ((buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF));
+  }
+
+  /**
+   * Switch to standard SPI mode.
+   *
+   * @return
+   */
+  private boolean 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) {
+        try {
+          Thread.sleep(10);
+        } catch (InterruptedException e) {
+        }
+      }
+      System.out.println("Paused the IMU processing thread successfully!");
+      // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
+      if (m_spi != null && 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.
+        int[] trashBuffer = new int[200];
+        try {
+          Thread.sleep(100);
+        } catch (InterruptedException e) {
+        }
+        int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
+        while (data_count > 0) {
+          m_spi.readAutoReceivedData(trashBuffer, Math.min(data_count, 200), 0);
+          data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
+        }
+        System.out.println("Paused auto SPI successfully.");
+      }
+    }
+    // There doesn't seem to be a SPI port active. Let's try to set one up
+    if (m_spi == null) {
+      System.out.println("Setting up a new SPI port.");
+      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
+      if (readRegister(PROD_ID) != 16982) {
+        DriverStation.reportError("Could not find ADIS16470", false);
+        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
+      if (readRegister(PROD_ID) != 16982) {
+        DriverStation.reportError("Could not find an ADIS16470", false);
+        close();
+        return false;
+      } else {
+        return true;
+      }
+    }
+  }
+
+  /** @return */
+  boolean switchToAutoSPI() {
+    // No SPI port has been set up. Go set one up first.
+    if (m_spi == null) {
+      if (!switchToStandardSPI()) {
+        DriverStation.reportError("Failed to start/restart auto SPI", false);
+        return false;
+      }
+    }
+    // Only set up the interrupt if needed.
+    if (m_auto_interrupt == null) {
+      // Configure interrupt on SPI CS1
+      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(5, 1000, 1);
+    // Kick off auto SPI (Note: Device configration impossible after auto SPI 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_acquire_task.isAlive()) {
+      m_first_run = true;
+      m_thread_active = true;
+      m_acquire_task.start();
+      System.out.println("Processing thread activated!");
+    } else {
+      // The thread was running, re-init run variables and start it up again.
+      m_first_run = true;
+      m_thread_active = true;
+      System.out.println("Processing thread activated!");
+    }
+    // Looks like the thread didn't start for some reason. Abort.
+    if (!m_acquire_task.isAlive()) {
+      DriverStation.reportError("Failed to start/restart the acquire() thread.", false);
+      close();
+      return false;
+    }
+    return true;
+  }
+
+  /**
+   * Configures calibration time
+   *
+   * @param new_cal_time New calibration time
+   * @return 1 if the new calibration time is the same as the current one else 0
+   */
+  public int configCalTime(CalibrationTime new_cal_time) {
+    if (m_calibration_time == new_cal_time.value) {
+      return 1;
+    }
+    if (!switchToStandardSPI()) {
+      DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
+      return 2;
+    }
+    m_calibration_time = new_cal_time.value;
+    writeRegister(NULL_CNFG, (m_calibration_time | 0x700));
+    if (!switchToAutoSPI()) {
+      DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
+      return 2;
+    }
+    return 0;
+  }
+
+  public int configDecRate(int reg) {
+    int m_reg = reg;
+    if (!switchToStandardSPI()) {
+      DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
+      return 2;
+    }
+    if (m_reg > 1999) {
+      DriverStation.reportError("Attemted to write an invalid deimation value.", false);
+      m_reg = 1999;
+    }
+    m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
+    writeRegister(DEC_RATE, m_reg);
+    System.out.println("Decimation register: " + readRegister(DEC_RATE));
+    if (!switchToAutoSPI()) {
+      DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
+      return 2;
+    }
+    return 0;
+  }
+
+  /**
+   * Calibrate the gyro. It's important to make sure that the robot is not moving while the
+   * calibration is in progress, this is typically done when the robot is first turned on while it's
+   * sitting at rest before the match starts.
+   */
+  public void calibrate() {
+    if (!switchToStandardSPI()) {
+      DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
+    }
+    writeRegister(GLOB_CMD, 0x0001);
+    if (!switchToAutoSPI()) {
+      DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
+    }
+    ;
+  }
+
+  /**
+   * Sets the yaw axis
+   *
+   * @param yaw_axis The new yaw axis to use
+   * @return 1 if the new yaw axis is the same as the current one, 2 if the switch to Standard SPI
+   *     failed, else 0.
+   */
+  public int setYawAxis(IMUAxis yaw_axis) {
+    if (m_yaw_axis == yaw_axis) {
+      return 1;
+    }
+    if (!switchToStandardSPI()) {
+      DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
+      return 2;
+    }
+    m_yaw_axis = yaw_axis;
+    if (!switchToAutoSPI()) {
+      DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
+    }
+    return 0;
+  }
+
+  /**
+   * @param reg
+   * @return
+   */
+  private int readRegister(int reg) {
+    ByteBuffer buf = ByteBuffer.allocateDirect(2);
+    buf.order(ByteOrder.BIG_ENDIAN);
+    buf.put(0, (byte) (reg & 0x7f));
+    buf.put(1, (byte) 0);
+
+    m_spi.write(buf, 2);
+    m_spi.read(false, buf, 2);
+
+    return toUShort(buf);
+  }
+
+  /**
+   * @param reg
+   * @param val
+   */
+  private void writeRegister(int reg, int val) {
+    ByteBuffer buf = ByteBuffer.allocateDirect(2);
+    // low byte
+    buf.put(0, (byte) ((0x80 | reg)));
+    buf.put(1, (byte) (val & 0xff));
+    m_spi.write(buf, 2);
+    // high byte
+    buf.put(0, (byte) (0x81 | reg));
+    buf.put(1, (byte) (val >> 8));
+    m_spi.write(buf, 2);
+  }
+
+  /** {@inheritDoc} */
+  public void reset() {
+    synchronized (this) {
+      m_integ_angle = 0.0;
+    }
+  }
+
+  /** Delete (free) the spi port used for the IMU. */
+  @Override
+  public void close() {
+    if (m_thread_active) {
+      m_thread_active = false;
+      try {
+        if (m_acquire_task != null) {
+          m_acquire_task.join();
+          m_acquire_task = null;
+        }
+      } catch (InterruptedException e) {
+      }
+      if (m_spi != null) {
+        if (m_auto_configured) {
+          m_spi.stopAuto();
+        }
+        m_spi.close();
+        m_auto_configured = false;
+        if (m_auto_interrupt != null) {
+          m_auto_interrupt.close();
+          m_auto_interrupt = null;
+        }
+        m_spi = null;
+      }
+    }
+    System.out.println("Finished cleaning up after the IMU driver.");
+  }
+
+  /** */
+  private void acquire() {
+    // Set data packet length
+    final int dataset_len = 19; // 18 data points + timestamp
+    final int BUFFER_SIZE = 4000;
+
+    // Set up buffers and variables
+    int[] buffer = new int[BUFFER_SIZE];
+    int data_count = 0;
+    int data_remainder = 0;
+    int data_to_read = 0;
+    double previous_timestamp = 0.0;
+    double delta_angle = 0.0;
+    double gyro_x = 0.0;
+    double gyro_y = 0.0;
+    double gyro_z = 0.0;
+    double accel_x = 0.0;
+    double accel_y = 0.0;
+    double accel_z = 0.0;
+    double gyro_x_si = 0.0;
+    double gyro_y_si = 0.0;
+    double gyro_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
+      try {
+        Thread.sleep(10);
+      } catch (InterruptedException e) {
+      }
+
+      if (m_thread_active) {
+        m_thread_idle = false;
+
+        data_count =
+            m_spi.readAutoReceivedData(
+                buffer, 0, 0); // 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) {
+          DriverStation.reportWarning(
+              "ADIS16470 data processing thread overrun has occurred!", false);
+          data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+        }
+        m_spi.readAutoReceivedData(
+            buffer, data_to_read, 0); // 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 = ((double) buffer[i] - previous_timestamp) / 1000000.0;
+
+          /*
+           * System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5],
+           * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] -
+           * previous_timestamp)) / 100.0));
+           * // DEBUG: Plot Sub-Array Data in Terminal
+           * for (int j = 0; j < data_to_read; j++) {
+           * System.out.print(buffer[j]);
+           * System.out.print(" ,");
+           * }
+           * System.out.println(" ");
+           * //System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5],
+           * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] -
+           * previous_timestamp)) / 100.0) + "," + buffer[3] + "," + buffer[4] + "," +
+           * buffer[5] + "," + buffer[6]
+           * /*toShort(buffer[7], buffer[8]) + "," +
+           * toShort(buffer[9], buffer[10]) + "," +
+           * toShort(buffer[11], buffer[12]) + "," +
+           * toShort(buffer[13], buffer[14]) + "," +
+           * toShort(buffer[15], buffer[16]) + ","
+           * + toShort(buffer[17], buffer[18]));
+           */
+
+          /*
+           * Get delta angle value for selected yaw axis and scale by the elapsed time
+           * (based on timestamp)
+           */
+          delta_angle =
+              (toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf)
+                  / (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+          gyro_x = (toShort(buffer[i + 7], buffer[i + 8]) / 10.0);
+          gyro_y = (toShort(buffer[i + 9], buffer[i + 10]) / 10.0);
+          gyro_z = (toShort(buffer[i + 11], buffer[i + 12]) / 10.0);
+          accel_x = (toShort(buffer[i + 13], buffer[i + 14]) / 800.0);
+          accel_y = (toShort(buffer[i + 15], buffer[i + 16]) / 800.0);
+          accel_z = (toShort(buffer[i + 17], buffer[i + 18]) / 800.0);
+
+          // Convert scaled sensor data to SI units (for tilt calculations)
+          // TODO: Should the unit outputs be selectable?
+          gyro_x_si = gyro_x * deg_to_rad;
+          gyro_y_si = gyro_y * deg_to_rad;
+          gyro_z_si = gyro_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) {
+            // Set up inclinometer calculations for first run
+            accelAngleX =
+                Math.atan2(
+                    accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
+            accelAngleY =
+                Math.atan2(
+                    accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
+            compAngleX = accelAngleX;
+            compAngleY = accelAngleY;
+          } else {
+            // Run inclinometer calculations
+            accelAngleX =
+                Math.atan2(
+                    accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
+            accelAngleY =
+                Math.atan2(
+                    accel_y_si, Math.sqrt((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_y_si);
+            compAngleY = compFilterProcess(compAngleY, accelAngleY, gyro_x_si);
+          }
+
+          synchronized (this) {
+            /* 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_x = gyro_x;
+            m_gyro_y = gyro_y;
+            m_gyro_z = gyro_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_x = 0.0;
+        gyro_y = 0.0;
+        gyro_z = 0.0;
+        accel_x = 0.0;
+        accel_y = 0.0;
+        accel_z = 0.0;
+        gyro_x_si = 0.0;
+        gyro_y_si = 0.0;
+        gyro_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;
+      }
+    }
+  }
+
+  /**
+   * @param compAngle
+   * @param accAngle
+   * @return
+   */
+  private double formatFastConverge(double compAngle, double accAngle) {
+    if (compAngle > accAngle + Math.PI) {
+      compAngle = compAngle - 2.0 * Math.PI;
+    } else if (accAngle > compAngle + Math.PI) {
+      compAngle = compAngle + 2.0 * Math.PI;
+    }
+    return compAngle;
+  }
+
+  /**
+   * @param compAngle
+   * @return
+   */
+  private double formatRange0to2PI(double compAngle) {
+    while (compAngle >= 2 * Math.PI) {
+      compAngle = compAngle - 2.0 * Math.PI;
+    }
+    while (compAngle < 0.0) {
+      compAngle = compAngle + 2.0 * Math.PI;
+    }
+    return compAngle;
+  }
+
+  /**
+   * @param accelAngle
+   * @param accelZ
+   * @return
+   */
+  private double formatAccelRange(double accelAngle, double accelZ) {
+    if (accelZ < 0.0) {
+      accelAngle = Math.PI - accelAngle;
+    } else if (accelZ > 0.0 && accelAngle < 0.0) {
+      accelAngle = 2.0 * Math.PI + accelAngle;
+    }
+    return accelAngle;
+  }
+
+  /**
+   * @param compAngle
+   * @param accelAngle
+   * @param omega
+   * @return
+   */
+  private double 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 > Math.PI) {
+      compAngle = compAngle - 2.0 * Math.PI;
+    }
+    return compAngle;
+  }
+
+  /** @return Yaw axis angle in degrees (CCW positive) */
+  public synchronized double getAngle() {
+    switch (m_yaw_axis) {
+      case kX:
+        if (m_simGyroAngleX != null) {
+          return m_simGyroAngleX.get();
+        }
+        break;
+      case kY:
+        if (m_simGyroAngleY != null) {
+          return m_simGyroAngleY.get();
+        }
+        break;
+      case kZ:
+        if (m_simGyroAngleZ != null) {
+          return m_simGyroAngleZ.get();
+        }
+        break;
+    }
+    return m_integ_angle;
+  }
+
+  /** @return Yaw Axis */
+  public IMUAxis getYawAxis() {
+    return m_yaw_axis;
+  }
+
+  /** @return current acceleration in the X axis */
+  public synchronized double getAccelX() {
+    return m_accel_x * 9.81;
+  }
+
+  /** @return current acceleration in the Y axis */
+  public synchronized double getAccelY() {
+    return m_accel_y * 9.81;
+  }
+
+  /** @return current acceleration in the Z axis */
+  public synchronized double getAccelZ() {
+    return m_accel_z * 9.81;
+  }
+
+  /** @return X axis complementary angle */
+  public synchronized double getXComplementaryAngle() {
+    return m_compAngleX;
+  }
+
+  /** @return Y axis complementary angle */
+  public synchronized double getYComplementaryAngle() {
+    return m_compAngleY;
+  }
+
+  /** @return X axis filtered acceleration angle */
+  public synchronized double getXFilteredAccelAngle() {
+    return m_accelAngleX;
+  }
+
+  /** @return Y axis filtered acceleration angle */
+  public synchronized double getYFilteredAccelAngle() {
+    return m_accelAngleY;
+  }
+
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  public int getPort() {
+    return m_spi_port.value;
+  }
+
+  @Override
+  public void initSendable(NTSendableBuilder builder) {
+    builder.setSmartDashboardType("Gyro");
+    builder.addDoubleProperty("Value", this::getAngle, null);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java

@@ -126,6 +126,16 @@ public class Compressor implements Sendable, AutoCloseable {
     return m_module.getAnalogVoltage(0);
   }
 
+  /**
+   * Query the analog sensor pressure (on channel 0) (if supported). Note this is only for use with
+   * the REV Analog Pressure Sensor.
+   *
+   * @return The analog sensor pressure, in PSI
+   */
+  public double getPressure() {
+    return m_module.getPressure(0);
+  }
+
   /** Disable the compressor. */
   public void disable() {
     m_module.disableCompressor();
@@ -137,27 +147,29 @@ public class Compressor implements Sendable, AutoCloseable {
   }
 
   /**
-   * Enable compressor closed loop control using analog input.
+   * 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 minAnalogVoltage The minimum voltage to enable compressor
-   * @param maxAnalogVoltage The maximum voltage to disable compressor
+   * @param minPressure The minimum pressure in PSI to enable compressor
+   * @param maxPressure The maximum pressure in PSI to disable compressor
    */
-  public void enableAnalog(double minAnalogVoltage, double maxAnalogVoltage) {
-    m_module.enableCompressorAnalog(minAnalogVoltage, maxAnalogVoltage);
+  public void enableAnalog(double minPressure, double maxPressure) {
+    m_module.enableCompressorAnalog(minPressure, maxPressure);
   }
 
   /**
-   * Enable compressor closed loop control using hybrid input.
+   * Enable compressor closed loop control using hybrid input. Note this is only for use with the
+   * REV Analog Pressure Sensor.
    *
    * <p>On CTRE PCM, this will enable digital control.
    *
-   * @param minAnalogVoltage The minimum voltage to enable compressor
-   * @param maxAnalogVoltage The maximum voltage to disable compressor
+   * @param minPressure The minimum pressure in PSI to enable compressor
+   * @param maxPressure The maximum pressure in PSI to disable compressor
    */
-  public void enableHybrid(double minAnalogVoltage, double maxAnalogVoltage) {
-    m_module.enableCompressorHybrid(minAnalogVoltage, maxAnalogVoltage);
+  public void enableHybrid(double minPressure, double maxPressure) {
+    m_module.enableCompressorHybrid(minPressure, maxPressure);
   }
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -1107,6 +1107,7 @@ public class DriverStation {
       return true;
     } catch (InterruptedException ex) {
       // return false on a thread interrupt
+      Thread.currentThread().interrupt();
       return false;
     } finally {
       m_waitForDataMutex.unlock();

wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java

@@ -43,6 +43,12 @@ public class I2C implements AutoCloseable {
     m_port = port.value;
     m_deviceAddress = deviceAddress;
 
+    if (port == I2C.Port.kOnboard) {
+      DriverStation.reportWarning(
+          "Onboard I2C port is subject to system lockups. See Known Issues page for details",
+          false);
+    }
+
     I2CJNI.i2CInitialize((byte) port.value);
 
     HAL.report(tResourceType.kResourceType_I2C, deviceAddress);

wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java

@@ -27,6 +27,17 @@ public class PneumaticHub implements PneumaticsBase {
       m_handle = REVPHJNI.initialize(module);
       m_module = module;
       m_handleMap.put(module, this);
+
+      final REVPHVersion version = REVPHJNI.getVersion(m_handle);
+      if (version.firmwareMajor > 0 && version.firmwareMajor < 22) {
+        final String fwVersion =
+            version.firmwareMajor + "." + version.firmwareMinor + "." + version.firmwareFix;
+        throw new IllegalStateException(
+            "The Pneumatic Hub has firmware version "
+                + fwVersion
+                + ", and must be updated to version 2022.0.0 or later "
+                + "using the REV Hardware Client.");
+      }
     }
 
     @Override
@@ -68,6 +79,18 @@ public class PneumaticHub implements PneumaticsBase {
     }
   }
 
+  /** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
+  private static double voltsToPsi(double sensorVoltage, double supplyVoltage) {
+    double pressure = 250 * (sensorVoltage / supplyVoltage) - 25;
+    return pressure;
+  }
+
+  /** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
+  private static double psiToVolts(double pressure, double supplyVoltage) {
+    double voltage = supplyVoltage * (0.004 * pressure + 0.1);
+    return voltage;
+  }
+
   private final DataStore m_dataStore;
   private final int m_handle;
 
@@ -210,12 +233,38 @@ public class PneumaticHub implements PneumaticsBase {
   }
 
   @Override
-  public void enableCompressorAnalog(double minAnalogVoltage, double maxAnalogVoltage) {
+  public void enableCompressorAnalog(double minPressure, double maxPressure) {
+    if (minPressure >= maxPressure) {
+      throw new IllegalArgumentException("maxPressure must be greater than minPressure");
+    }
+    if (minPressure < 0 || minPressure > 120) {
+      throw new IllegalArgumentException(
+          "minPressure must be between 0 and 120 PSI, got " + minPressure);
+    }
+    if (maxPressure < 0 || maxPressure > 120) {
+      throw new IllegalArgumentException(
+          "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
+    }
+    double minAnalogVoltage = psiToVolts(minPressure, 5);
+    double maxAnalogVoltage = psiToVolts(maxPressure, 5);
     REVPHJNI.setClosedLoopControlAnalog(m_handle, minAnalogVoltage, maxAnalogVoltage);
   }
 
   @Override
-  public void enableCompressorHybrid(double minAnalogVoltage, double maxAnalogVoltage) {
+  public void enableCompressorHybrid(double minPressure, double maxPressure) {
+    if (minPressure >= maxPressure) {
+      throw new IllegalArgumentException("maxPressure must be greater than minPressure");
+    }
+    if (minPressure < 0 || minPressure > 120) {
+      throw new IllegalArgumentException(
+          "minPressure must be between 0 and 120 PSI, got " + minPressure);
+    }
+    if (maxPressure < 0 || maxPressure > 120) {
+      throw new IllegalArgumentException(
+          "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
+    }
+    double minAnalogVoltage = psiToVolts(minPressure, 5);
+    double maxAnalogVoltage = psiToVolts(maxPressure, 5);
     REVPHJNI.setClosedLoopControlHybrid(m_handle, minAnalogVoltage, maxAnalogVoltage);
   }
 
@@ -224,6 +273,13 @@ public class PneumaticHub implements PneumaticsBase {
     return REVPHJNI.getAnalogVoltage(m_handle, channel);
   }
 
+  @Override
+  public double getPressure(int channel) {
+    double sensorVoltage = REVPHJNI.getAnalogVoltage(m_handle, channel);
+    double supplyVoltage = REVPHJNI.get5VVoltage(m_handle);
+    return voltsToPsi(sensorVoltage, supplyVoltage);
+  }
+
   void clearStickyFaults() {
     REVPHJNI.clearStickyFaults(m_handle);
   }

wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java

@@ -90,12 +90,14 @@ public interface PneumaticsBase extends AutoCloseable {
 
   void enableCompressorDigital();
 
-  void enableCompressorAnalog(double minAnalogVoltage, double maxAnalogVoltage);
+  void enableCompressorAnalog(double minPressure, double maxPressure);
 
-  void enableCompressorHybrid(double minAnalogVoltage, double maxAnalogVoltage);
+  void enableCompressorHybrid(double minPressure, double maxPressure);
 
   double getAnalogVoltage(int channel);
 
+  double getPressure(int channel);
+
   CompressorConfigType getCompressorConfigType();
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java

@@ -235,12 +235,12 @@ public class PneumaticsControlModule implements PneumaticsBase {
   }
 
   @Override
-  public void enableCompressorAnalog(double minAnalogVoltage, double maxAnalogVoltage) {
+  public void enableCompressorAnalog(double minPressure, double maxPressure) {
     CTREPCMJNI.setClosedLoopControl(m_handle, false);
   }
 
   @Override
-  public void enableCompressorHybrid(double minAnalogVoltage, double maxAnalogVoltage) {
+  public void enableCompressorHybrid(double minPressure, double maxPressure) {
     CTREPCMJNI.setClosedLoopControl(m_handle, false);
   }
 
@@ -255,4 +255,9 @@ public class PneumaticsControlModule implements PneumaticsBase {
   public double getAnalogVoltage(int channel) {
     return 0;
   }
+
+  @Override
+  public double getPressure(int channel) {
+    return 0;
+  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java

@@ -55,6 +55,8 @@ public class Servo extends PWM {
    * Get the servo position.
    *
    * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left 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.
    */
@@ -87,8 +89,8 @@ public class Servo extends PWM {
   /**
    * Get the servo angle.
    *
-   * <p>Assume that the servo angle is linear with respect to the PWM value (big assumption, need to
-   * test).
+   * <p>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.
    */

wpilibj/src/main/java/edu/wpi/first/wpilibj/counter/Tachometer.java

@@ -99,13 +99,13 @@ public class Tachometer implements Sendable, AutoCloseable {
   }
 
   /**
-   * Gets the current tachometer revolution per minute.
+   * Gets the current tachometer revolutions per second.
    *
    * <p>setEdgesPerRevolution must be set with a non 0 value for this to return valid values.
    *
-   * @return Current RPM.
+   * @return Current RPS.
    */
-  public double getRevolutionsPerMinute() {
+  public double getRevolutionsPerSecond() {
     double period = getPeriod();
     if (period == 0) {
       return 0;
@@ -114,7 +114,18 @@ public class Tachometer implements Sendable, AutoCloseable {
     if (edgesPerRevolution == 0) {
       return 0;
     }
-    return ((1.0 / edgesPerRevolution) / period) * 60;
+    return (1.0 / edgesPerRevolution) / period;
+  }
+
+  /**
+   * Gets the current tachometer revolutions per minute.
+   *
+   * <p>setEdgesPerRevolution must be set with a non 0 value for this to return valid values.
+   *
+   * @return Current RPM.
+   */
+  public double getRevolutionsPerMinute() {
+    return getRevolutionsPerSecond() * 60;
   }
 
   /**
@@ -165,6 +176,7 @@ public class Tachometer implements Sendable, AutoCloseable {
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Tachometer");
+    builder.addDoubleProperty("RPS", this::getRevolutionsPerSecond, null);
     builder.addDoubleProperty("RPM", this::getRevolutionsPerMinute, null);
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java

@@ -5,17 +5,19 @@
 package edu.wpi.first.wpilibj.shuffleboard;
 
 import edu.wpi.first.cscore.VideoSource;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
 import java.util.Map;
+import java.util.Objects;
 import java.util.WeakHashMap;
 
 /** A wrapper to make video sources sendable and usable from Shuffleboard. */
 public final class SendableCameraWrapper implements Sendable, AutoCloseable {
   private static final String kProtocol = "camera_server://";
 
-  private static Map<VideoSource, SendableCameraWrapper> m_wrappers = new WeakHashMap<>();
+  private static Map<String, SendableCameraWrapper> m_wrappers = new WeakHashMap<>();
 
   private final String m_uri;
 
@@ -26,9 +28,18 @@ public final class SendableCameraWrapper implements Sendable, AutoCloseable {
    * @param source the source to wrap
    */
   private SendableCameraWrapper(VideoSource source) {
-    String name = source.getName();
-    SendableRegistry.add(this, name);
-    m_uri = kProtocol + name;
+    this(source.getName());
+  }
+
+  private SendableCameraWrapper(String cameraName) {
+    SendableRegistry.add(this, cameraName);
+    m_uri = kProtocol + cameraName;
+  }
+
+  /** Clears all cached wrapper objects. This should only be used in tests. */
+  @SuppressWarnings("PMD.DefaultPackage")
+  static void clearWrappers() {
+    m_wrappers.clear();
   }
 
   @Override
@@ -45,7 +56,52 @@ public final class SendableCameraWrapper implements Sendable, AutoCloseable {
    *     ShuffleboardTab#add(Sendable)} and {@link ShuffleboardLayout#add(Sendable)}
    */
   public static SendableCameraWrapper wrap(VideoSource source) {
-    return m_wrappers.computeIfAbsent(source, SendableCameraWrapper::new);
+    return m_wrappers.computeIfAbsent(source.getName(), name -> new SendableCameraWrapper(source));
+  }
+
+  /**
+   * Creates a wrapper for an arbitrary camera stream. The stream URLs <i>must</i> be specified
+   * using a host resolvable by a program running on a different host (such as a dashboard); prefer
+   * using static IP addresses (if known) or DHCP identifiers such as {@code "raspberrypi.local"}.
+   *
+   * <p>If a wrapper already exists for the given camera, that wrapper is returned and the specified
+   * URLs are ignored.
+   *
+   * @param cameraName the name of the camera. Cannot be null or empty
+   * @param cameraUrls the URLs with which the camera stream may be accessed. At least one URL must
+   *     be specified
+   * @return a sendable wrapper object for the video source, usable in Shuffleboard via {@link
+   *     ShuffleboardTab#add(Sendable)} and {@link ShuffleboardLayout#add(Sendable)}
+   */
+  @SuppressWarnings("PMD.CyclomaticComplexity")
+  public static SendableCameraWrapper wrap(String cameraName, String... cameraUrls) {
+    if (m_wrappers.containsKey(cameraName)) {
+      return m_wrappers.get(cameraName);
+    }
+
+    Objects.requireNonNull(cameraName, "cameraName");
+    Objects.requireNonNull(cameraUrls, "cameraUrls");
+    if (cameraName.isEmpty()) {
+      throw new IllegalArgumentException("Camera name not specified");
+    }
+    if (cameraUrls.length == 0) {
+      throw new IllegalArgumentException("No camera URLs specified");
+    }
+    for (int i = 0; i < cameraUrls.length; i++) {
+      Objects.requireNonNull(cameraUrls[i], "Camera URL at index " + i + " was null");
+    }
+
+    String streams = "/CameraPublisher/" + cameraName + "/streams";
+    if (NetworkTableInstance.getDefault().getEntries(streams, 0).length != 0) {
+      throw new IllegalStateException(
+          "A camera is already being streamed with the name '" + cameraName + "'");
+    }
+
+    NetworkTableInstance.getDefault().getEntry(streams).setStringArray(cameraUrls);
+
+    SendableCameraWrapper wrapper = new SendableCameraWrapper(cameraName);
+    m_wrappers.put(cameraName, wrapper);
+    return wrapper;
   }
 
   @Override

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java

@@ -122,6 +122,20 @@ public interface ShuffleboardContainer extends ShuffleboardValue {
    */
   SimpleWidget add(String title, Object defaultValue);
 
+  /**
+   * 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
+   * @throws IllegalArgumentException if a widget already exists in this container with the given
+   *     title
+   */
+  default ComplexWidget addCamera(String title, String cameraName, String... cameraUrls) {
+    return add(title, SendableCameraWrapper.wrap(cameraName, cameraUrls));
+  }
+
   /**
    * Adds a widget to this container. The widget will display the data provided by the value
    * supplier. Changes made on the dashboard will not propagate to the widget object, and will be

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java

@@ -0,0 +1,88 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.ADIS16448_IMU;
+
+/** Class to control a simulated ADIS16448 gyroscope. */
+@SuppressWarnings({"TypeName", "AbbreviationAsWordInName"})
+public class ADIS16448_IMUSim {
+  private final SimDouble m_simGyroAngleX;
+  private final SimDouble m_simGyroAngleY;
+  private final SimDouble m_simGyroAngleZ;
+  private final SimDouble m_simAccelX;
+  private final SimDouble m_simAccelY;
+  private final SimDouble m_simAccelZ;
+
+  /**
+   * Constructs from an ADIS16448_IMU object.
+   *
+   * @param gyro ADIS16448_IMU to simulate
+   */
+  public ADIS16448_IMUSim(ADIS16448_IMU gyro) {
+    SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADIS16448" + "[" + gyro.getPort() + "]");
+    m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
+    m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
+    m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
+    m_simAccelX = wrappedSimDevice.getDouble("accel_x");
+    m_simAccelY = wrappedSimDevice.getDouble("accel_y");
+    m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
+  }
+
+  /**
+   * Sets the X axis angle in degrees (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleX(double angleDegrees) {
+    m_simGyroAngleX.set(angleDegrees);
+  }
+
+  /**
+   * Sets the Y axis angle in degrees (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleY(double angleDegrees) {
+    m_simGyroAngleY.set(angleDegrees);
+  }
+
+  /**
+   * Sets the Z axis angle in degrees (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleZ(double angleDegrees) {
+    m_simGyroAngleZ.set(angleDegrees);
+  }
+
+  /**
+   * Sets the X axis acceleration in meters per second squared.
+   *
+   * @param accelMetersPerSecondSquared The acceleration.
+   */
+  public void setAccelX(double accelMetersPerSecondSquared) {
+    m_simAccelX.set(accelMetersPerSecondSquared);
+  }
+
+  /**
+   * Sets the Y axis acceleration in meters per second squared.
+   *
+   * @param accelMetersPerSecondSquared The acceleration.
+   */
+  public void setAccelY(double accelMetersPerSecondSquared) {
+    m_simAccelY.set(accelMetersPerSecondSquared);
+  }
+
+  /**
+   * Sets the Z axis acceleration in meters per second squared.
+   *
+   * @param accelMetersPerSecondSquared The acceleration.
+   */
+  public void setAccelZ(double accelMetersPerSecondSquared) {
+    m_simAccelZ.set(accelMetersPerSecondSquared);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java

@@ -0,0 +1,88 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.ADIS16470_IMU;
+
+/** Class to control a simulated ADIS16470 gyroscope. */
+@SuppressWarnings({"TypeName", "AbbreviationAsWordInName"})
+public class ADIS16470_IMUSim {
+  private final SimDouble m_simGyroAngleX;
+  private final SimDouble m_simGyroAngleY;
+  private final SimDouble m_simGyroAngleZ;
+  private final SimDouble m_simAccelX;
+  private final SimDouble m_simAccelY;
+  private final SimDouble m_simAccelZ;
+
+  /**
+   * Constructs from an ADIS16470_IMU object.
+   *
+   * @param gyro ADIS16470_IMU to simulate
+   */
+  public ADIS16470_IMUSim(ADIS16470_IMU gyro) {
+    SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADIS16470" + "[" + gyro.getPort() + "]");
+    m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
+    m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
+    m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
+    m_simAccelX = wrappedSimDevice.getDouble("accel_x");
+    m_simAccelY = wrappedSimDevice.getDouble("accel_y");
+    m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
+  }
+
+  /**
+   * Sets the X axis angle in degrees (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleX(double angleDegrees) {
+    m_simGyroAngleX.set(angleDegrees);
+  }
+
+  /**
+   * Sets the Y axis angle in degrees (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleY(double angleDegrees) {
+    m_simGyroAngleY.set(angleDegrees);
+  }
+
+  /**
+   * Sets the Z axis angle in degrees (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleZ(double angleDegrees) {
+    m_simGyroAngleZ.set(angleDegrees);
+  }
+
+  /**
+   * Sets the X axis acceleration in meters per second squared.
+   *
+   * @param accelMetersPerSecondSquared The acceleration.
+   */
+  public void setAccelX(double accelMetersPerSecondSquared) {
+    m_simAccelX.set(accelMetersPerSecondSquared);
+  }
+
+  /**
+   * Sets the Y axis acceleration in meters per second squared.
+   *
+   * @param accelMetersPerSecondSquared The acceleration.
+   */
+  public void setAccelY(double accelMetersPerSecondSquared) {
+    m_simAccelY.set(accelMetersPerSecondSquared);
+  }
+
+  /**
+   * Sets the Z axis acceleration in meters per second squared.
+   *
+   * @param accelMetersPerSecondSquared The acceleration.
+   */
+  public void setAccelZ(double accelMetersPerSecondSquared) {
+    m_simAccelZ.set(accelMetersPerSecondSquared);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java

@@ -360,7 +360,7 @@ public class DifferentialDrivetrainSim {
    * @return The normalized input.
    */
   protected Matrix<N2, N1> clampInput(Matrix<N2, N1> u) {
-    return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage());
+    return StateSpaceUtil.desaturateInputVector(u, RobotController.getBatteryVoltage());
   }
 
   /** Represents the different states of the drivetrain. */

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java

@@ -186,6 +186,6 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
    * @return The normalized input.
    */
   protected Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> u) {
-    return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage());
+    return StateSpaceUtil.desaturateInputVector(u, RobotController.getBatteryVoltage());
   }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapperTest.java

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import org.junit.jupiter.api.AfterAll;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class SendableCameraWrapperTest {
+  @BeforeEach
+  void setup() {
+    NetworkTableInstance.getDefault().deleteAllEntries();
+    SendableCameraWrapper.clearWrappers();
+  }
+
+  @AfterAll
+  static void tearDown() {
+    NetworkTableInstance.getDefault().deleteAllEntries();
+  }
+
+  @Test
+  void testNullCameraName() {
+    assertThrows(NullPointerException.class, () -> SendableCameraWrapper.wrap(null, ""));
+  }
+
+  @Test
+  void testEmptyCameraName() {
+    assertThrows(IllegalArgumentException.class, () -> SendableCameraWrapper.wrap("", ""));
+  }
+
+  @Test
+  void testNullUrlArray() {
+    assertThrows(
+        NullPointerException.class, () -> SendableCameraWrapper.wrap("name", (String[]) null));
+  }
+
+  @Test
+  void testNullUrlInArray() {
+    assertThrows(NullPointerException.class, () -> SendableCameraWrapper.wrap("name", "url", null));
+  }
+
+  @Test
+  void testEmptyUrlArray() {
+    assertThrows(IllegalArgumentException.class, () -> SendableCameraWrapper.wrap("name"));
+  }
+
+  @Test
+  void testUrlsAddedToNt() {
+    SendableCameraWrapper.wrap("name", "url1", "url2");
+    assertArrayEquals(
+        new String[] {"url1", "url2"},
+        NetworkTableInstance.getDefault()
+            .getEntry("/CameraPublisher/name/streams")
+            .getValue()
+            .getStringArray());
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java

@@ -62,7 +62,7 @@ class DifferentialDrivetrainSimTest {
                 .addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1)));
 
     for (double t = 0; t < traj.getTotalTimeSeconds(); t += 0.020) {
-      var state = traj.sample(0.020);
+      var state = traj.sample(t);
       var ramseteOut = ramsete.calculate(sim.getPose(), state);
 
       var wheelSpeeds = kinematics.toWheelSpeeds(ramseteOut);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java

@@ -19,11 +19,19 @@ public class Robot extends TimedRobot {
   private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
   private final Joystick m_stick = new Joystick(0);
 
+  @Override
+  public void robotInit() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotor.setInverted(true);
+  }
+
   @Override
   public void teleopPeriodic() {
     // Drive with arcade drive.
     // That means that the Y axis drives forward
     // and backward, and the X turns left and right.
-    m_robotDrive.arcadeDrive(m_stick.getY(), m_stick.getX());
+    m_robotDrive.arcadeDrive(-m_stick.getY(), m_stick.getX());
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java

@@ -19,11 +19,19 @@ public class Robot extends TimedRobot {
   private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
   private final XboxController m_driverController = new XboxController(0);
 
+  @Override
+  public void robotInit() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotor.setInverted(true);
+  }
+
   @Override
   public void teleopPeriodic() {
     // Drive with split arcade drive.
     // That means that the Y axis of the left stick moves forward
     // and backward, and the X of the right stick turns left and right.
-    m_robotDrive.arcadeDrive(m_driverController.getLeftY(), m_driverController.getRightX());
+    m_robotDrive.arcadeDrive(-m_driverController.getLeftY(), m_driverController.getRightX());
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java

@@ -42,7 +42,7 @@ public class RobotContainer {
         new RunCommand(
             () ->
                 m_robotDrive.arcadeDrive(
-                    m_driverController.getLeftY(), m_driverController.getRightX()),
+                    -m_driverController.getLeftY(), m_driverController.getRightX()),
             m_robotDrive));
   }