[examples] Add communication examples (e.g. arduino) (#2500)

Co-authored-by: Andrew Dassonville <dassonville.andrew@gmail.com>

.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/I2C.cpp

@@ -54,6 +54,10 @@ void HAL_InitializeI2C(HAL_I2CPort port, int32_t* status) {
   }
 
   if (port == HAL_I2C_kOnboard) {
+    HAL_SendError(0, 0, 0,
+                  "Onboard I2C port is subject to system lockups. See Known "
+                  "Issues page for details",
+                  "", "", true);
     std::scoped_lock lock(digitalI2COnBoardMutex);
     i2COnboardObjCount++;
     if (i2COnboardObjCount > 1) {

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;

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogAccelerometer.java

@@ -9,7 +9,13 @@ import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDSourceType;
 
-/** Wrapper so that PIDSource is implemented for AnalogAccelerometer for old PIDController. */
+/**
+ * Wrapper so that PIDSource is implemented for AnalogAccelerometer for old PIDController.
+ *
+ * @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
+ *     wrapper.
+ */
+@Deprecated(since = "2022", forRemoval = true)
 public class PIDAnalogAccelerometer extends AnalogAccelerometer implements PIDSource {
   protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogGyro.java

@@ -9,7 +9,13 @@ import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDSourceType;
 
-/** Wrapper so that PIDSource is implemented for AnalogGyro for old PIDController. */
+/**
+ * Wrapper so that PIDSource is implemented for AnalogGyro for old PIDController.
+ *
+ * @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
+ *     wrapper.
+ */
+@Deprecated(since = "2022", forRemoval = true)
 public class PIDAnalogGyro extends AnalogGyro implements PIDSource {
   private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogInput.java

@@ -8,7 +8,13 @@ import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDSourceType;
 
-/** Wrapper so that PIDSource is implemented for AnalogInput for old PIDController. */
+/**
+ * Wrapper so that PIDSource is implemented for AnalogInput for old PIDController.
+ *
+ * @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
+ *     wrapper.
+ */
+@Deprecated(since = "2022", forRemoval = true)
 public class PIDAnalogInput extends AnalogInput implements PIDSource {
   protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogPotentiometer.java

@@ -9,7 +9,13 @@ import edu.wpi.first.wpilibj.AnalogPotentiometer;
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDSourceType;
 
-/** Wrapper so that PIDSource is implemented for AnalogPotentiometer for old PIDController. */
+/**
+ * Wrapper so that PIDSource is implemented for AnalogPotentiometer for old PIDController.
+ *
+ * @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
+ *     wrapper.
+ */
+@Deprecated(since = "2022", forRemoval = true)
 public class PIDAnalogPotentiometer extends AnalogPotentiometer implements PIDSource {
   protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDEncoder.java

@@ -9,7 +9,13 @@ import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDSourceType;
 
-/** Wrapper so that PIDSource is implemented for Encoder for old PIDController. */
+/**
+ * Wrapper so that PIDSource is implemented for Encoder for old PIDController.
+ *
+ * @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
+ *     wrapper.
+ */
+@Deprecated(since = "2022", forRemoval = true)
 public class PIDEncoder extends Encoder implements PIDSource {
   private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDMotorController.java

@@ -9,7 +9,13 @@ import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 
-/** Wrapper so that PIDOutput is implemented for MotorController for old PIDController. */
+/**
+ * Wrapper so that PIDOutput is implemented for MotorController for old PIDController.
+ *
+ * @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
+ *     wrapper.
+ */
+@Deprecated(since = "2022", forRemoval = true)
 public class PIDMotorController implements PIDOutput, MotorController, Sendable {
   private final MotorController m_motorController;
 

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDUltrasonic.java

@@ -10,7 +10,13 @@ import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDSourceType;
 import edu.wpi.first.wpilibj.Ultrasonic;
 
-/** Wrapper so that PIDSource is implemented for Ultrasonic for old PIDController. */
+/**
+ * Wrapper so that PIDSource is implemented for Ultrasonic for old PIDController.
+ *
+ * @deprecated Use {@link edu.wpi.first.math.controller.PIDController} which doesn't require this
+ *     wrapper.
+ */
+@Deprecated(since = "2022", forRemoval = true)
 public class PIDUltrasonic extends Ultrasonic implements PIDSource {
   protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 

wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogAccelerometer.h

@@ -12,8 +12,13 @@ namespace frc {
 /**
  * Wrapper so that PIDSource is implemented for AnalogAccelerometer for old
  * PIDController
+ *
+ * @deprecated Use frc2::PIDController class instead which doesn't require this
+ * wrapper.
  */
-class PIDAnalogAccelerometer : public PIDSource, public AnalogAccelerometer {
+class WPI_DEPRECATED("Use frc2::PIDController class instead.")
+    PIDAnalogAccelerometer : public PIDSource,
+                             public AnalogAccelerometer {
   using AnalogAccelerometer::AnalogAccelerometer;
 
  public:

wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogGyro.h

@@ -11,8 +11,13 @@ namespace frc {
 
 /**
  * Wrapper so that PIDSource is implemented for AnalogGyro for old PIDController
+ *
+ * @deprecated Use frc2::PIDController class instead which doesn't require this
+ * wrapper.
  */
-class PIDAnalogGyro : public PIDSource, public AnalogGyro {
+class WPI_DEPRECATED("Use frc2::PIDController class instead.") PIDAnalogGyro
+    : public PIDSource,
+      public AnalogGyro {
   using AnalogGyro::AnalogGyro;
 
  public:

wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogInput.h

@@ -12,8 +12,13 @@ namespace frc {
 /**
  * Wrapper so that PIDSource is implemented for AnalogInput for old
  * PIDController
+ *
+ * @deprecated Use frc2::PIDController class instead which doesn't require this
+ * wrapper.
  */
-class PIDAnalogInput : public PIDSource, public AnalogInput {
+class WPI_DEPRECATED("Use frc2::PIDController class instead.") PIDAnalogInput
+    : public PIDSource,
+      public AnalogInput {
   using AnalogInput::AnalogInput;
 
  public:

wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDAnalogPotentiometer.h

@@ -12,8 +12,14 @@ namespace frc {
 /**
  * Wrapper so that PIDSource is implemented for AnalogPotentiometer for old
  * PIDController
+ *
+ *
+ * @deprecated Use frc2::PIDController class instead which doesn't require this
+ * wrapper.
  */
-class PIDAnalogPotentiometer : public PIDSource, public AnalogPotentiometer {
+class WPI_DEPRECATED("Use frc2::PIDController class instead.")
+    PIDAnalogPotentiometer : public PIDSource,
+                             public AnalogPotentiometer {
   using AnalogPotentiometer::AnalogPotentiometer;
 
  public:

wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDEncoder.h

@@ -11,8 +11,13 @@ namespace frc {
 
 /**
  * Wrapper so that PIDSource is implemented for Encoder for old PIDController
+ *
+ * @deprecated Use frc2::PIDController class instead which doesn't require this
+ * wrapper.
  */
-class PIDEncoder : public PIDSource, public Encoder {
+class WPI_DEPRECATED("Use frc2::PIDController class instead.") PIDEncoder
+    : public PIDSource,
+      public Encoder {
   using Encoder::Encoder;
 
  public:

wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDMotorController.h

@@ -15,11 +15,15 @@ namespace frc {
 /**
  * Wrapper so that PIDOutput is implemented for MotorController for old
  * PIDController
+ *
+ * @deprecated Use frc2::PIDController class instead which doesn't require this
+ * wrapper.
  */
 class PIDMotorController : public PIDOutput,
                            public MotorController,
                            public wpi::Sendable {
  public:
+  WPI_DEPRECATED("Use frc2::PIDController class instead.")
   explicit PIDMotorController(MotorController& motorController);
 
   /**

wpilibOldCommands/src/main/native/include/frc/pidwrappers/PIDUltrasonic.h

@@ -11,8 +11,13 @@ namespace frc {
 
 /**
  * Wrapper so that PIDSource is implemented for Ultrasonic for old PIDController
+ *
+ * @deprecated Use frc2::PIDController class instead which doesn't require this
+ * wrapper.
  */
-class PIDUltrasonic : public PIDSource, public Ultrasonic {
+class WPI_DEPRECATED("Use frc2::PIDController class instead.") PIDUltrasonic
+    : public PIDSource,
+      public Ultrasonic {
   using Ultrasonic::Ultrasonic;
 
  public:

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

@@ -0,0 +1,910 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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_simGyroRateX =
+        m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
+    m_simGyroRateY =
+        m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
+    m_simGyroRateZ =
+        m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
+    m_simAccelX =
+        m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
+    m_simAccelY =
+        m_simDevice.CreateDouble("accel_y", hal::SimDevice::kInput, 0.0);
+    m_simAccelZ =
+        m_simDevice.CreateDouble("accel_z", hal::SimDevice::kInput, 0.0);
+  }
+
+  if (!m_simDevice) {
+    // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+    // Relies on the RIO hardware by default configuring an output as low
+    // and configuring an input as high Z. The 10k pull-up resistor internal to
+    // the IMU then forces the reset line high for normal operation.
+    DigitalOutput* m_reset_out = new DigitalOutput(18);  // Drive MXP DIO8 low
+    Wait(10_ms);                                         // Wait 10ms
+    delete m_reset_out;
+    new DigitalInput(18);  // Set MXP DIO8 high
+    Wait(500_ms);          // Wait 500ms for reset to complete
+
+    ConfigCalTime(cal_time);
+
+    // Configure standard SPI
+    if (!SwitchToStandardSPI()) {
+      return;
+    }
+
+    // Set IMU internal decimation to 819.2 SPS
+    WriteRegister(SMPL_PRD, 0x0001);
+    // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
+    WriteRegister(MSC_CTRL, 0x0016);
+    // Disable IMU internal Bartlett filter
+    WriteRegister(SENS_AVG, 0x0400);
+    // Clear offset registers
+    WriteRegister(XGYRO_OFF, 0x0000);
+    WriteRegister(YGYRO_OFF, 0x0000);
+    WriteRegister(ZGYRO_OFF, 0x0000);
+    // Configure and enable auto SPI
+    if (!SwitchToAutoSPI()) {
+      return;
+    }
+    // Notify DS that IMU calibration delay is active
+    REPORT_WARNING(
+        "ADIS16448 IMU Detected. Starting initial calibration delay.");
+    // Wait for whatever time the user set as the start-up delay
+    Wait(static_cast<double>(m_calibration_time) * 1.2_s);
+    // Execute calibration routine
+    Calibrate();
+    // Reset accumulated offsets
+    Reset();
+    // Tell the acquire loop that we're done starting up
+    m_start_up_mode = false;
+
+    // Let the user know the IMU was initiallized successfully
+    REPORT_WARNING("ADIS16448 IMU Successfully Initialized!");
+
+    // TODO: Find what the proper pin is to turn this LED
+    //  Drive MXP PWM5 (IMU ready LED) low (active low)
+    new DigitalOutput(19);
+  }
+
+  // Report usage and post data to DS
+  HAL_Report(HALUsageReporting::kResourceType_ADIS16448, 0);
+
+  wpi::SendableRegistry::AddLW(this, "ADIS16448", port);
+}
+
+/**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in standard SPI mode.
+ *
+ * This function switches the active SPI port to standard SPI and is used
+ *primarily when exiting auto SPI. Exiting auto SPI is required to read or write
+ *using SPI since the auto SPI configuration, once active, locks the SPI message
+ *being transacted. This function also verifies that the SPI port is operating
+ *in standard SPI mode by reading back the IMU product ID.
+ **/
+bool ADIS16448_IMU::SwitchToStandardSPI() {
+  // Check to see whether the acquire thread is active. If so, wait for it to
+  // stop producing data.
+  if (m_thread_active) {
+    m_thread_active = false;
+    while (!m_thread_idle) {
+      Wait(10_ms);
+    }
+    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 OffsetData[size];
+
+  // set accumulate count to 0
+  m_accum_count = 0;
+}
+
+/**
+ * This function switches the active SPI port to auto SPI and is used primarily
+ *when exiting standard SPI. Auto SPI is required to asynchronously read data
+ *over SPI as it utilizes special FPGA hardware to react to an external data
+ *ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
+ *can be read by the CPU asynchronously. Standard SPI transactions are
+ * impossible on the selected SPI port once auto SPI is enabled. The stall
+ *settings, GPIO interrupt pin, and data packet settings used in this function
+ *are hard-coded to work only with the ADIS16448 IMU.
+ **/
+bool ADIS16448_IMU::SwitchToAutoSPI() {
+  // No SPI port has been set up. Go set one up first.
+  if (m_spi == nullptr) {
+    if (!SwitchToStandardSPI()) {
+      REPORT_ERROR("Failed to start/restart auto SPI");
+      return false;
+    }
+  }
+  // Only set up the interrupt if needed.
+  if (m_auto_interrupt == nullptr) {
+    m_auto_interrupt = new DigitalInput(10);
+  }
+  // The auto SPI controller gets angry if you try to set up two instances on
+  // one bus.
+  if (!m_auto_configured) {
+    m_spi->InitAuto(8200);
+    m_auto_configured = true;
+  }
+  // Set auto SPI packet data and size
+  m_spi->SetAutoTransmitData({{GLOB_CMD}}, 27);
+  // Configure auto stall time
+  m_spi->ConfigureAutoStall(HAL_SPI_kMXP, 100, 1000, 255);
+  // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
+  // activated)
+  m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
+  // Check to see if the acquire thread is running. If not, kick one off.
+  if (!m_thread_idle) {
+    m_first_run = true;
+    m_thread_active = true;
+    // Set up circular buffer
+    InitOffsetBuffer(m_avg_size);
+    // Kick off acquire thread
+    m_acquire_task = std::thread(&ADIS16448_IMU::Acquire, this);
+    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 accum_gyro_rate_x = 0.0;
+  double accum_gyro_rate_y = 0.0;
+  double accum_gyro_rate_z = 0.0;
+  for (int i = 0; i < gyroAverageSize; i++) {
+    accum_gyro_rate_x += m_offset_buffer[i].gyro_rate_x;
+    accum_gyro_rate_y += m_offset_buffer[i].gyro_rate_y;
+    accum_gyro_rate_z += m_offset_buffer[i].gyro_rate_z;
+  }
+  m_gyro_rate_offset_x = accum_gyro_rate_x / gyroAverageSize;
+  m_gyro_rate_offset_y = accum_gyro_rate_y / gyroAverageSize;
+  m_gyro_rate_offset_z = accum_gyro_rate_z / gyroAverageSize;
+  m_integ_gyro_angle_x = 0.0;
+  m_integ_gyro_angle_y = 0.0;
+  m_integ_gyro_angle_z = 0.0;
+  // std::cout << "Avg Size: " << gyroAverageSize << " X off: " <<
+  // m_gyro_rate_offset_x << " Y off: " << m_gyro_rate_offset_y << " Z off: " <<
+  // m_gyro_rate_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_angle_x = 0.0;
+  m_integ_gyro_angle_y = 0.0;
+  m_integ_gyro_angle_z = 0.0;
+}
+
+void ADIS16448_IMU::Close() {
+  if (m_thread_active) {
+    m_thread_active = false;
+    if (m_acquire_task.joinable()) {
+      m_acquire_task.join();
+    }
+  }
+  if (m_spi != nullptr) {
+    if (m_auto_configured) {
+      m_spi->StopAuto();
+    }
+    delete m_spi;
+    m_auto_configured = false;
+    if (m_auto_interrupt != nullptr) {
+      delete m_auto_interrupt;
+      m_auto_interrupt = nullptr;
+    }
+    m_spi = nullptr;
+  }
+  delete[] m_offset_buffer;
+  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;
+
+  // This buffer can contain many datasets
+  uint32_t buffer[BUFFER_SIZE];
+  int data_count = 0;
+  int data_remainder = 0;
+  int data_to_read = 0;
+  int bufferAvgIndex = 0;
+  uint32_t previous_timestamp = 0;
+  double gyro_rate_x = 0.0;
+  double gyro_rate_y = 0.0;
+  double gyro_rate_z = 0.0;
+  double accel_x = 0.0;
+  double accel_y = 0.0;
+  double accel_z = 0.0;
+  double mag_x = 0.0;
+  double mag_y = 0.0;
+  double mag_z = 0.0;
+  double baro = 0.0;
+  double temp = 0.0;
+  double gyro_rate_x_si = 0.0;
+  double gyro_rate_y_si = 0.0;
+  // double gyro_rate_z_si = 0.0;
+  double accel_x_si = 0.0;
+  double accel_y_si = 0.0;
+  double accel_z_si = 0.0;
+  double compAngleX = 0.0;
+  double compAngleY = 0.0;
+  double accelAngleX = 0.0;
+  double accelAngleY = 0.0;
+
+  while (true) {
+    // Sleep loop for 10ms (wait for data)
+    Wait(10_ms);
+
+    if (m_thread_active) {
+      data_count = m_spi->ReadAutoReceivedData(
+          buffer, 0,
+          0_s);  // Read number of bytes currently stored in the buffer
+      data_remainder =
+          data_count % dataset_len;  // Check if frame is incomplete
+      data_to_read = data_count -
+                     data_remainder;  // Remove incomplete data from read count
+      /* Want to cap the data to read in a single read at the buffer size */
+      if (data_to_read > BUFFER_SIZE) {
+        REPORT_WARNING(
+            "ADIS16448 data processing thread overrun has occurred!");
+        data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+      }
+      m_spi->ReadAutoReceivedData(buffer, data_to_read,
+                                  0_s);  // Read data from DMA buffer
+
+      // Could be multiple data sets in the buffer. Handle each one.
+      for (int i = 0; i < data_to_read; i += dataset_len) {
+        // Calculate CRC-16 on each data packet
+        uint16_t calc_crc = 0xFFFF;  // Starting word
+        uint8_t byte = 0;
+        uint16_t imu_crc = 0;
+        for (int k = 5; k < 27;
+             k += 2)  // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP
+                      // (Ignore Status & CRC)
+        {
+          byte = BuffToUByte(&buffer[i + k + 1]);  // Process LSB
+          calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
+          byte = BuffToUByte(&buffer[i + k]);  // Process MSB
+          calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
+        }
+        calc_crc = ~calc_crc;  // Complement
+        calc_crc = static_cast<uint16_t>((calc_crc << 8) |
+                                         (calc_crc >> 8));  // Flip LSB & MSB
+        imu_crc =
+            BuffToUShort(&buffer[i + 27]);  // Extract DUT CRC from data buffer
+
+        // Compare calculated vs read CRC. Don't update outputs or dt if CRC-16
+        // is bad
+        if (calc_crc == imu_crc) {
+          // Timestamp is at buffer[i]
+          m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
+          // Split array and scale data
+          gyro_rate_x = BuffToShort(&buffer[i + 5]) * 0.04;
+          gyro_rate_y = BuffToShort(&buffer[i + 7]) * 0.04;
+          gyro_rate_z = BuffToShort(&buffer[i + 9]) * 0.04;
+          accel_x = BuffToShort(&buffer[i + 11]) * 0.833;
+          accel_y = BuffToShort(&buffer[i + 13]) * 0.833;
+          accel_z = BuffToShort(&buffer[i + 15]) * 0.833;
+          mag_x = BuffToShort(&buffer[i + 17]) * 0.1429;
+          mag_y = BuffToShort(&buffer[i + 19]) * 0.1429;
+          mag_z = BuffToShort(&buffer[i + 21]) * 0.1429;
+          baro = BuffToShort(&buffer[i + 23]) * 0.02;
+          temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0;
+
+          /*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_rate_x_si = gyro_rate_x * deg_to_rad;
+          gyro_rate_y_si = gyro_rate_y * deg_to_rad;
+          // gyro_rate_z_si = gyro_rate_z * deg_to_rad;
+          accel_x_si = accel_x * grav;
+          accel_y_si = accel_y * grav;
+          accel_z_si = accel_z * grav;
+          // Store timestamp for next iteration
+          previous_timestamp = buffer[i];
+          // Calculate alpha for use with the complementary filter
+          m_alpha = m_tau / (m_tau + m_dt);
+          // Calculate complementary filter
+          if (m_first_run) {
+            accelAngleX = atan2f(
+                -accel_x_si,
+                sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+            accelAngleY =
+                atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
+                                         (-accel_z_si * -accel_z_si)));
+            compAngleX = accelAngleX;
+            compAngleY = accelAngleY;
+          } else {
+            accelAngleX = atan2f(
+                -accel_x_si,
+                sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+            accelAngleY =
+                atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
+                                         (-accel_z_si * -accel_z_si)));
+            accelAngleX = FormatAccelRange(accelAngleX, -accel_z_si);
+            accelAngleY = FormatAccelRange(accelAngleY, -accel_z_si);
+            compAngleX =
+                CompFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
+            compAngleY =
+                CompFilterProcess(compAngleY, accelAngleY, -gyro_rate_x_si);
+          }
+
+          // Update global variables and state
+          {
+            std::scoped_lock sync(m_mutex);
+            // Ignore first, integrated sample
+            if (m_first_run) {
+              m_integ_gyro_angle_x = 0.0;
+              m_integ_gyro_angle_y = 0.0;
+              m_integ_gyro_angle_z = 0.0;
+            } else {
+              // Accumulate gyro for offset calibration
+              // Add most recent sample data to buffer
+              bufferAvgIndex = m_accum_count % m_avg_size;
+              m_offset_buffer[bufferAvgIndex] =
+                  OffsetData{gyro_rate_x, gyro_rate_y, gyro_rate_z};
+              // Increment counter
+              m_accum_count++;
+            }
+            // Don't post accumulated data to the global variables until an
+            // initial gyro offset has been calculated
+            if (!m_start_up_mode) {
+              m_gyro_rate_x = gyro_rate_x;
+              m_gyro_rate_y = gyro_rate_y;
+              m_gyro_rate_z = gyro_rate_z;
+              m_accel_x = accel_x;
+              m_accel_y = accel_y;
+              m_accel_z = accel_z;
+              m_mag_x = mag_x;
+              m_mag_y = mag_y;
+              m_mag_z = mag_z;
+              m_baro = baro;
+              m_temp = temp;
+              m_compAngleX = compAngleX * rad_to_deg;
+              m_compAngleY = compAngleY * rad_to_deg;
+              m_accelAngleX = accelAngleX * rad_to_deg;
+              m_accelAngleY = accelAngleY * rad_to_deg;
+              // Accumulate gyro for angle integration and publish to global
+              // variables
+              m_integ_gyro_angle_x +=
+                  (gyro_rate_x - m_gyro_rate_offset_x) * m_dt;
+              m_integ_gyro_angle_y +=
+                  (gyro_rate_y - m_gyro_rate_offset_y) * m_dt;
+              m_integ_gyro_angle_z +=
+                  (gyro_rate_z - m_gyro_rate_offset_z) * m_dt;
+            }
+          }
+          m_first_run = false;
+        } else {
+          /*
+          // 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_rate_x = 0.0;
+      gyro_rate_y = 0.0;
+      gyro_rate_z = 0.0;
+      accel_x = 0.0;
+      accel_y = 0.0;
+      accel_z = 0.0;
+      mag_x = 0.0;
+      mag_y = 0.0;
+      mag_z = 0.0;
+      baro = 0.0;
+      temp = 0.0;
+      gyro_rate_x_si = 0.0;
+      gyro_rate_y_si = 0.0;
+      // gyro_rate_z_si = 0.0;
+      accel_x_si = 0.0;
+      accel_y_si = 0.0;
+      accel_z_si = 0.0;
+      compAngleX = 0.0;
+      compAngleY = 0.0;
+      accelAngleX = 0.0;
+      accelAngleY = 0.0;
+    }
+  }
+}
+
+/* Complementary filter functions */
+double ADIS16448_IMU::FormatFastConverge(double compAngle, double accAngle) {
+  if (compAngle > accAngle + wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  } else if (accAngle > compAngle + wpi::numbers::pi) {
+    compAngle = compAngle + 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+double ADIS16448_IMU::FormatRange0to2PI(double compAngle) {
+  while (compAngle >= 2 * wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  }
+  while (compAngle < 0.0) {
+    compAngle = compAngle + 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+double ADIS16448_IMU::FormatAccelRange(double accelAngle, double accelZ) {
+  if (accelZ < 0.0) {
+    accelAngle = wpi::numbers::pi - accelAngle;
+  } else if (accelZ > 0.0 && accelAngle < 0.0) {
+    accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
+  }
+  return accelAngle;
+}
+
+double ADIS16448_IMU::CompFilterProcess(double compAngle, double accelAngle,
+                                        double omega) {
+  compAngle = FormatFastConverge(compAngle, accelAngle);
+  compAngle =
+      m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+  compAngle = FormatRange0to2PI(compAngle);
+  if (compAngle > wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+int ADIS16448_IMU::ConfigDecRate(uint16_t DecimationSetting) {
+  uint16_t writeValue = DecimationSetting;
+  uint16_t readbackValue;
+  if (!SwitchToStandardSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+    return 2;
+  }
+
+  /* Check max */
+  if (DecimationSetting > 9) {
+    REPORT_ERROR("Attemted to write an invalid decimation value. Capping at 9");
+    DecimationSetting = 9;
+  }
+
+  /* Shift decimation setting to correct position and select internal sync */
+  writeValue = (DecimationSetting << 8) | 0x1;
+
+  /* Apply to IMU */
+  WriteRegister(SMPL_PRD, writeValue);
+
+  /* Perform read back to verify write */
+  readbackValue = ReadRegister(SMPL_PRD);
+
+  /* Throw error for invalid write */
+  if (readbackValue != writeValue) {
+    REPORT_ERROR("ADIS16448 SMPL_PRD write failed.");
+  }
+
+  if (!SwitchToAutoSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+    return 2;
+  }
+  return 0;
+}
+
+units::degree_t ADIS16448_IMU::GetAngle() const {
+  switch (m_yaw_axis) {
+    case kX:
+      return GetGyroAngleX();
+    case kY:
+      return GetGyroAngleY();
+    case kZ:
+      return GetGyroAngleZ();
+    default:
+      return 0_deg;
+  }
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetRate() const {
+  switch (m_yaw_axis) {
+    case kX:
+      return GetGyroRateX();
+    case kY:
+      return GetGyroRateY();
+    case kZ:
+      return GetGyroRateZ();
+    default:
+      return 0_deg_per_s;
+  }
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleX() const {
+  if (m_simGyroAngleX) {
+    return units::degree_t{m_simGyroAngleX.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_integ_gyro_angle_x};
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleY() const {
+  if (m_simGyroAngleY) {
+    return units::degree_t{m_simGyroAngleY.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_integ_gyro_angle_y};
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleZ() const {
+  if (m_simGyroAngleZ) {
+    return units::degree_t{m_simGyroAngleZ.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_integ_gyro_angle_z};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateX() const {
+  if (m_simGyroRateX) {
+    return units::degrees_per_second_t{m_simGyroRateX.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degrees_per_second_t{m_gyro_rate_x};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateY() const {
+  if (m_simGyroRateY) {
+    return units::degrees_per_second_t{m_simGyroRateY.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degrees_per_second_t{m_gyro_rate_y};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateZ() const {
+  if (m_simGyroRateZ) {
+    return units::degrees_per_second_t{m_simGyroRateZ.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degrees_per_second_t{m_gyro_rate_z};
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelX() const {
+  if (m_simAccelX) {
+    return units::meters_per_second_squared_t{m_simAccelX.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return m_accel_x * 9.81_mps_sq;
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelY() const {
+  if (m_simAccelY) {
+    return units::meters_per_second_squared_t{m_simAccelY.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return m_accel_y * 9.81_mps_sq;
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelZ() const {
+  if (m_simAccelZ) {
+    return units::meters_per_second_squared_t{m_simAccelZ.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return m_accel_z * 9.81_mps_sq;
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldX() const {
+  std::scoped_lock sync(m_mutex);
+  return units::gauss_t{m_mag_x * 1e-3};
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldY() const {
+  std::scoped_lock sync(m_mutex);
+  return units::gauss_t{m_mag_y * 1e-3};
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldZ() const {
+  std::scoped_lock sync(m_mutex);
+  return units::gauss_t{m_mag_z * 1e-3};
+}
+
+units::degree_t ADIS16448_IMU::GetXComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleX};
+}
+
+units::degree_t ADIS16448_IMU::GetYComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleY};
+}
+
+units::degree_t ADIS16448_IMU::GetXFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleX};
+}
+
+units::degree_t ADIS16448_IMU::GetYFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleY};
+}
+
+units::pounds_per_square_inch_t ADIS16448_IMU::GetBarometricPressure() const {
+  std::scoped_lock sync(m_mutex);
+  return units::mbar_t{m_baro};
+}
+
+units::celsius_t ADIS16448_IMU::GetTemperature() const {
+  std::scoped_lock sync(m_mutex);
+  return units::celsius_t{m_temp};
+}
+
+ADIS16448_IMU::IMUAxis ADIS16448_IMU::GetYawAxis() const {
+  return m_yaw_axis;
+}
+
+int ADIS16448_IMU::SetYawAxis(IMUAxis yaw_axis) {
+  if (m_yaw_axis == yaw_axis) {
+    return 1;
+  } else {
+    m_yaw_axis = yaw_axis;
+    Reset();
+    return 0;
+  }
+}
+
+int ADIS16448_IMU::GetPort() const {
+  return m_spi_port;
+}
+
+/**
+ * @brief Builds a Sendable object to push IMU data to the driver station.
+ *
+ * This function pushes the most recent angle estimates for all axes to the
+ *driver station.
+ **/
+void ADIS16448_IMU::InitSendable(nt::NTSendableBuilder& builder) {
+  builder.SetSmartDashboardType("ADIS16448 IMU");
+  auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
+  builder.SetUpdateTable([=]() {
+    nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
+  });
+}

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

@@ -0,0 +1,837 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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_simGyroRateX =
+        m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
+    m_simGyroRateY =
+        m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
+    m_simGyroRateZ =
+        m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
+    m_simAccelX =
+        m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
+    m_simAccelY =
+        m_simDevice.CreateDouble("accel_y", hal::SimDevice::kInput, 0.0);
+    m_simAccelZ =
+        m_simDevice.CreateDouble("accel_z", hal::SimDevice::kInput, 0.0);
+  }
+
+  if (!m_simDevice) {
+    // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+    // Relies on the RIO hardware by default configuring an output as low
+    // and configuring an input as high Z. The 10k pull-up resistor internal to
+    // the IMU then forces the reset line high for normal operation.
+    DigitalOutput* m_reset_out =
+        new DigitalOutput(27);  // Drive SPI CS2 (IMU RST) low
+    Wait(10_ms);                // Wait 10ms
+    delete m_reset_out;
+    new DigitalInput(27);  // Set SPI CS2 (IMU RST) high
+    Wait(500_ms);          // Wait 500ms for reset to complete
+
+    // Configure standard SPI
+    if (!SwitchToStandardSPI()) {
+      return;
+    }
+
+    // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1)
+    // = 400Hz)
+    WriteRegister(DEC_RATE, 0x0004);
+    // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation
+    // and PoP
+    WriteRegister(MSC_CTRL, 0x0001);
+    // Configure IMU internal Bartlett filter
+    WriteRegister(FILT_CTRL, 0x0000);
+    // Configure continuous bias calibration time based on user setting
+    WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
+
+    // Notify DS that IMU calibration delay is active
+    REPORT_WARNING(
+        "ADIS16470 IMU Detected. Starting initial calibration delay.");
+
+    // Wait for samples to accumulate internal to the IMU (110% of user-defined
+    // time)
+    Wait(units::second_t{pow(2, m_calibration_time) / 2000 * 64 * 1.1});
+
+    // Write offset calibration command to IMU
+    WriteRegister(GLOB_CMD, 0x0001);
+
+    // Configure and enable auto SPI
+    if (!SwitchToAutoSPI()) {
+      return;
+    }
+
+    // Let the user know the IMU was initiallized successfully
+    REPORT_WARNING("ADIS16470 IMU Successfully Initialized!");
+
+    // Drive SPI CS3 (IMU ready LED) low (active low)
+    new DigitalOutput(28);
+  }
+
+  // Report usage and post data to DS
+  HAL_Report(HALUsageReporting::kResourceType_ADIS16470, 0);
+
+  wpi::SendableRegistry::AddLW(this, "ADIS16470", port);
+}
+
+/**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in standard SPI mode.
+ *
+ * This function switches the active SPI port to standard SPI and is used
+ *primarily when exiting auto SPI. Exiting auto SPI is required to read or write
+ *using SPI since the auto SPI configuration, once active, locks the SPI message
+ *being transacted. This function also verifies that the SPI port is operating
+ *in standard SPI mode by reading back the IMU product ID.
+ **/
+bool ADIS16470_IMU::SwitchToStandardSPI() {
+  // Check to see whether the acquire thread is active. If so, wait for it to
+  // stop producing data.
+  if (m_thread_active) {
+    m_thread_active = false;
+    while (!m_thread_idle) {
+      Wait(10_ms);
+    }
+    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_rate_x = 0.0;
+  double gyro_rate_y = 0.0;
+  double gyro_rate_z = 0.0;
+  double accel_x = 0.0;
+  double accel_y = 0.0;
+  double accel_z = 0.0;
+  double gyro_rate_x_si = 0.0;
+  double gyro_rate_y_si = 0.0;
+  // double gyro_rate_z_si = 0.0;
+  double accel_x_si = 0.0;
+  double accel_y_si = 0.0;
+  double accel_z_si = 0.0;
+  double compAngleX = 0.0;
+  double compAngleY = 0.0;
+  double accelAngleX = 0.0;
+  double accelAngleY = 0.0;
+
+  while (true) {
+    // Sleep loop for 10ms (wait for data)
+    Wait(10_ms);
+
+    if (m_thread_active) {
+      m_thread_idle = false;
+
+      data_count = m_spi->ReadAutoReceivedData(
+          buffer, 0,
+          0_s);  // Read number of bytes currently stored in the buffer
+      data_remainder =
+          data_count % dataset_len;  // Check if frame is incomplete. Add 1
+                                     // because of timestamp
+      data_to_read = data_count -
+                     data_remainder;  // Remove incomplete data from read count
+      /* Want to cap the data to read in a single read at the buffer size */
+      if (data_to_read > BUFFER_SIZE) {
+        REPORT_WARNING(
+            "ADIS16470 data processing thread overrun has occurred!");
+        data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+      }
+      m_spi->ReadAutoReceivedData(
+          buffer, data_to_read,
+          0_s);  // Read data from DMA buffer (only complete sets)
+
+      /*
+      // 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_rate_x = (BuffToShort(&buffer[i + 7]) / 10.0);
+        gyro_rate_y = (BuffToShort(&buffer[i + 9]) / 10.0);
+        gyro_rate_z = (BuffToShort(&buffer[i + 11]) / 10.0);
+        accel_x = (BuffToShort(&buffer[i + 13]) / 800.0);
+        accel_y = (BuffToShort(&buffer[i + 15]) / 800.0);
+        accel_z = (BuffToShort(&buffer[i + 17]) / 800.0);
+
+        // Convert scaled sensor data to SI units
+        gyro_rate_x_si = gyro_rate_x * deg_to_rad;
+        gyro_rate_y_si = gyro_rate_y * deg_to_rad;
+        // gyro_rate_z_si = gyro_rate_z * deg_to_rad;
+        accel_x_si = accel_x * grav;
+        accel_y_si = accel_y * grav;
+        accel_z_si = accel_z * grav;
+
+        // Store timestamp for next iteration
+        previous_timestamp = buffer[i];
+
+        /*
+        // 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_rate_y_si);
+          compAngleY =
+              CompFilterProcess(compAngleY, accelAngleY, gyro_rate_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_rate_x = gyro_rate_x;
+          m_gyro_rate_y = gyro_rate_y;
+          m_gyro_rate_z = gyro_rate_z;
+          m_accel_x = accel_x;
+          m_accel_y = accel_y;
+          m_accel_z = accel_z;
+          m_compAngleX = compAngleX * rad_to_deg;
+          m_compAngleY = compAngleY * rad_to_deg;
+          m_accelAngleX = accelAngleX * rad_to_deg;
+          m_accelAngleY = accelAngleY * rad_to_deg;
+        }
+        m_first_run = false;
+      }
+    } else {
+      m_thread_idle = true;
+      data_count = 0;
+      data_remainder = 0;
+      data_to_read = 0;
+      previous_timestamp = 0.0;
+      delta_angle = 0.0;
+      gyro_rate_x = 0.0;
+      gyro_rate_y = 0.0;
+      gyro_rate_z = 0.0;
+      accel_x = 0.0;
+      accel_y = 0.0;
+      accel_z = 0.0;
+      gyro_rate_x_si = 0.0;
+      gyro_rate_y_si = 0.0;
+      // gyro_rate_z_si = 0.0;
+      accel_x_si = 0.0;
+      accel_y_si = 0.0;
+      accel_z_si = 0.0;
+      compAngleX = 0.0;
+      compAngleY = 0.0;
+      accelAngleX = 0.0;
+      accelAngleY = 0.0;
+    }
+  }
+}
+
+/* Complementary filter functions */
+double ADIS16470_IMU::FormatFastConverge(double compAngle, double accAngle) {
+  if (compAngle > accAngle + wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  } else if (accAngle > compAngle + wpi::numbers::pi) {
+    compAngle = compAngle + 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+double ADIS16470_IMU::FormatRange0to2PI(double compAngle) {
+  while (compAngle >= 2 * wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  }
+  while (compAngle < 0.0) {
+    compAngle = compAngle + 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+double ADIS16470_IMU::FormatAccelRange(double accelAngle, double accelZ) {
+  if (accelZ < 0.0) {
+    accelAngle = wpi::numbers::pi - accelAngle;
+  } else if (accelZ > 0.0 && accelAngle < 0.0) {
+    accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
+  }
+  return accelAngle;
+}
+
+double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
+                                        double omega) {
+  compAngle = FormatFastConverge(compAngle, accelAngle);
+  compAngle =
+      m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+  compAngle = FormatRange0to2PI(compAngle);
+  if (compAngle > wpi::numbers::pi) {
+    compAngle = compAngle - 2.0 * wpi::numbers::pi;
+  }
+  return compAngle;
+}
+
+units::degree_t ADIS16470_IMU::GetAngle() const {
+  switch (m_yaw_axis) {
+    case kX:
+      if (m_simGyroAngleX) {
+        return units::degree_t{m_simGyroAngleX.Get()};
+      }
+      break;
+    case kY:
+      if (m_simGyroAngleY) {
+        return units::degree_t{m_simGyroAngleY.Get()};
+      }
+      break;
+    case kZ:
+      if (m_simGyroAngleZ) {
+        return units::degree_t{m_simGyroAngleZ.Get()};
+      }
+      break;
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_integ_angle};
+}
+
+units::degrees_per_second_t ADIS16470_IMU::GetRate() const {
+  if (m_yaw_axis == kX) {
+    if (m_simGyroRateX) {
+      return units::degrees_per_second_t{m_simGyroRateX.Get()};
+    }
+    std::scoped_lock sync(m_mutex);
+    return units::degrees_per_second_t{m_gyro_rate_x};
+  } else if (m_yaw_axis == kY) {
+    if (m_simGyroRateY) {
+      return units::degrees_per_second_t{m_simGyroRateY.Get()};
+    }
+    std::scoped_lock sync(m_mutex);
+    return units::degrees_per_second_t{m_gyro_rate_y};
+  } else if (m_yaw_axis == kZ) {
+    if (m_simGyroRateZ) {
+      return units::degrees_per_second_t{m_simGyroRateZ.Get()};
+    }
+    std::scoped_lock sync(m_mutex);
+    return units::degrees_per_second_t{m_gyro_rate_z};
+  } else {
+    return 0_deg_per_s;
+  }
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const {
+  if (m_simAccelX) {
+    return units::meters_per_second_squared_t{m_simAccelX.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::meters_per_second_squared_t{m_accel_x};
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelY() const {
+  if (m_simAccelY) {
+    return units::meters_per_second_squared_t{m_simAccelY.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::meters_per_second_squared_t{m_accel_y};
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelZ() const {
+  if (m_simAccelZ) {
+    return units::meters_per_second_squared_t{m_simAccelZ.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::meters_per_second_squared_t{m_accel_z};
+}
+
+units::degree_t ADIS16470_IMU::GetXComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleX};
+}
+
+units::degree_t ADIS16470_IMU::GetYComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleY};
+}
+
+units::degree_t ADIS16470_IMU::GetXFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleX};
+}
+
+units::degree_t ADIS16470_IMU::GetYFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleY};
+}
+
+ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const {
+  return m_yaw_axis;
+}
+
+int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) {
+  if (m_yaw_axis == yaw_axis) {
+    return 1;
+  }
+  if (!SwitchToStandardSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+    return 2;
+  }
+  m_yaw_axis = yaw_axis;
+  if (!SwitchToAutoSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+    return 2;
+  }
+  return 0;
+}
+
+int ADIS16470_IMU::GetPort() const {
+  return m_spi_port;
+}
+
+/**
+ * @brief Builds a Sendable object to push IMU data to the driver station.
+ *
+ * This function pushes the most recent angle estimates for all axes to the
+ *driver station.
+ **/
+void ADIS16470_IMU::InitSendable(nt::NTSendableBuilder& builder) {
+  builder.SetSmartDashboardType("ADIS16470 IMU");
+  auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
+  builder.SetUpdateTable([=]() {
+    nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
+  });
+}

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,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/ADIS16448_IMUSim.h"
+
+#include <frc/ADIS16448_IMU.h>
+#include <frc/simulation/SimDeviceSim.h>
+
+using namespace frc::sim;
+
+ADIS16448_IMUSim::ADIS16448_IMUSim(const frc::ADIS16448_IMU& imu) {
+  frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16448", imu.GetPort()};
+  m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
+  m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
+  m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
+  m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
+  m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
+  m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
+  m_simAccelX = deviceSim.GetDouble("accel_x");
+  m_simAccelY = deviceSim.GetDouble("accel_y");
+  m_simAccelZ = deviceSim.GetDouble("accel_z");
+}
+
+void ADIS16448_IMUSim::SetGyroAngleX(units::degree_t angle) {
+  m_simGyroAngleX.Set(angle.value());
+}
+
+void ADIS16448_IMUSim::SetGyroAngleY(units::degree_t angle) {
+  m_simGyroAngleY.Set(angle.value());
+}
+
+void ADIS16448_IMUSim::SetGyroAngleZ(units::degree_t angle) {
+  m_simGyroAngleZ.Set(angle.value());
+}
+
+void ADIS16448_IMUSim::SetGyroRateX(units::degrees_per_second_t angularRate) {
+  m_simGyroRateX.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) {
+  m_simGyroRateY.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) {
+  m_simGyroRateZ.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
+  m_simAccelX.Set(accel.value());
+}
+
+void ADIS16448_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
+  m_simAccelY.Set(accel.value());
+}
+
+void ADIS16448_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
+  m_simAccelZ.Set(accel.value());
+}

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

@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/ADIS16470_IMUSim.h"
+
+#include <frc/ADIS16470_IMU.h>
+#include <frc/simulation/SimDeviceSim.h>
+
+using namespace frc::sim;
+
+ADIS16470_IMUSim::ADIS16470_IMUSim(const frc::ADIS16470_IMU& imu) {
+  frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16470", imu.GetPort()};
+  m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
+  m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
+  m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
+  m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
+  m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
+  m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
+  m_simAccelX = deviceSim.GetDouble("accel_x");
+  m_simAccelY = deviceSim.GetDouble("accel_y");
+  m_simAccelZ = deviceSim.GetDouble("accel_z");
+}
+
+void ADIS16470_IMUSim::SetGyroAngleX(units::degree_t angle) {
+  m_simGyroAngleX.Set(angle.value());
+}
+
+void ADIS16470_IMUSim::SetGyroAngleY(units::degree_t angle) {
+  m_simGyroAngleY.Set(angle.value());
+}
+
+void ADIS16470_IMUSim::SetGyroAngleZ(units::degree_t angle) {
+  m_simGyroAngleZ.Set(angle.value());
+}
+
+void ADIS16470_IMUSim::SetGyroRateX(units::degrees_per_second_t angularRate) {
+  m_simGyroRateX.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) {
+  m_simGyroRateY.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) {
+  m_simGyroRateZ.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
+  m_simAccelX.Set(accel.value());
+}
+
+void ADIS16470_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
+  m_simAccelY.Set(accel.value());
+}
+
+void ADIS16470_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
+  m_simAccelZ.Set(accel.value());
+}

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,428 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. All Rights Reserved.           */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*                                                                            */
+/* Modified by Juan Chong - frcsupport@analog.com                             */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+#include <atomic>
+#include <memory>
+#include <thread>
+
+#include <hal/SimDevice.h>
+#include <networktables/NTSendable.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/magnetic_field_strength.h>
+#include <units/pressure.h>
+#include <units/temperature.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+#include "frc/DigitalSource.h"
+#include "frc/SPI.h"
+
+namespace frc {
+/**
+ * Use DMA SPI to read rate, acceleration, and magnetometer data from the
+ * ADIS16448 IMU and return the robots heading relative to a starting position,
+ * AHRS, and instant measurements
+ *
+ * The ADIS16448 gyro angle outputs track the robot's heading based on the
+ * starting position. As the robot rotates the new heading is computed by
+ * integrating the rate of rotation returned by the IMU. When the class is
+ * instantiated, a short calibration routine is performed where the IMU samples
+ * the gyros while at rest to determine the initial offset. This is subtracted
+ * from each sample to determine the heading.
+ *
+ * This class is for the ADIS16448 IMU connected via the SPI port available on
+ * the RoboRIO MXP port.
+ */
+
+class ADIS16448_IMU : public nt::NTSendable,
+                      public wpi::SendableHelper<ADIS16448_IMU> {
+ public:
+  /* ADIS16448 Calibration Time Enum Class */
+  enum class CalibrationTime {
+    _32ms = 0,
+    _64ms = 1,
+    _128ms = 2,
+    _256ms = 3,
+    _512ms = 4,
+    _1s = 5,
+    _2s = 6,
+    _4s = 7,
+    _8s = 8,
+    _16s = 9,
+    _32s = 10,
+    _64s = 11
+  };
+
+  enum IMUAxis { kX, kY, kZ };
+
+  /**
+   * IMU constructor on onboard MXP CS0, Z-up orientation, and complementary
+   * AHRS computation.
+   */
+  ADIS16448_IMU();
+
+  /**
+   * IMU constructor on the specified MXP port and orientation.
+   *
+   * @param yaw_axis The axis where gravity is present. Valid options are kX,
+   * kY, and kZ
+   * @param port The SPI port where the IMU is connected.
+   * @param cal_time The calibration time that should be used on start-up.
+   */
+  explicit ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
+                         CalibrationTime cal_time);
+
+  ~ADIS16448_IMU() override;
+
+  ADIS16448_IMU(ADIS16448_IMU&&) = default;
+  ADIS16448_IMU& operator=(ADIS16448_IMU&&) = default;
+
+  /**
+   * Initialize the IMU.
+   *
+   * Perform gyro offset calibration by collecting data for a number of seconds
+   * and computing the center value. The center value is subtracted from
+   * subsequent measurements.
+   *
+   * It's important to make sure that the robot is not moving while the
+   * centering calculations are in progress, this is typically done when the
+   * robot is first turned on while it's sitting at rest before the match
+   * starts.
+   *
+   * The calibration routine can be triggered by the user during runtime.
+   */
+  void Calibrate();
+
+  /**
+   * Configures the calibration time used for the next calibrate.
+   *
+   * @param new_cal_time The calibration time that should be used
+   */
+  int ConfigCalTime(CalibrationTime new_cal_time);
+
+  /**
+   * Reset the gyro.
+   *
+   * Resets the gyro accumulations to a heading of zero. This can be used if
+   * there is significant drift in the gyro and it needs to be recalibrated
+   * after running.
+   */
+  void Reset();
+
+  /**
+   * Returns the yaw axis angle in degrees (CCW positive).
+   */
+  units::degree_t GetAngle() const;
+
+  /**
+   * Returns the yaw axis angular rate in degrees per second (CCW positive).
+   */
+  units::degrees_per_second_t GetRate() const;
+
+  /**
+   * Returns the accumulated gyro angle in the X axis.
+   */
+  units::degree_t GetGyroAngleX() const;
+
+  /**
+   * Returns the accumulated gyro angle in the Y axis.
+   */
+  units::degree_t GetGyroAngleY() const;
+
+  /**
+   * Returns the accumulated gyro angle in the Z axis.
+   */
+  units::degree_t GetGyroAngleZ() const;
+
+  /**
+   * Returns the angular rate in the X axis.
+   */
+  units::degrees_per_second_t GetGyroRateX() const;
+
+  /**
+   * Returns the angular rate in the Y axis.
+   */
+  units::degrees_per_second_t GetGyroRateY() const;
+
+  /**
+   * Returns the angular rate in the Z axis.
+   */
+  units::degrees_per_second_t GetGyroRateZ() const;
+
+  /**
+   * Returns the acceleration in the X axis.
+   */
+  units::meters_per_second_squared_t GetAccelX() const;
+
+  /**
+   * Returns the acceleration in the Y axis.
+   */
+  units::meters_per_second_squared_t GetAccelY() const;
+
+  /**
+   * Returns the acceleration in the Z axis.
+   */
+  units::meters_per_second_squared_t GetAccelZ() const;
+
+  units::degree_t GetXComplementaryAngle() const;
+
+  units::degree_t GetYComplementaryAngle() const;
+
+  units::degree_t GetXFilteredAccelAngle() const;
+
+  units::degree_t GetYFilteredAccelAngle() const;
+
+  units::tesla_t GetMagneticFieldX() const;
+
+  units::tesla_t GetMagneticFieldY() const;
+
+  units::tesla_t GetMagneticFieldZ() const;
+
+  units::pounds_per_square_inch_t GetBarometricPressure() const;
+
+  units::celsius_t GetTemperature() const;
+
+  IMUAxis GetYawAxis() const;
+
+  int SetYawAxis(IMUAxis yaw_axis);
+
+  int ConfigDecRate(uint16_t DecimationRate);
+
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  int GetPort() const;
+
+  void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+  /** @brief ADIS16448 Register Map Declaration */
+  static constexpr uint8_t FLASH_CNT = 0x00;  // Flash memory write count
+  static constexpr uint8_t XGYRO_OUT = 0x04;  // X-axis gyroscope output
+  static constexpr uint8_t YGYRO_OUT = 0x06;  // Y-axis gyroscope output
+  static constexpr uint8_t ZGYRO_OUT = 0x08;  // Z-axis gyroscope output
+  static constexpr uint8_t XACCL_OUT = 0x0A;  // X-axis accelerometer output
+  static constexpr uint8_t YACCL_OUT = 0x0C;  // Y-axis accelerometer output
+  static constexpr uint8_t ZACCL_OUT = 0x0E;  // Z-axis accelerometer output
+  static constexpr uint8_t XMAGN_OUT = 0x10;  // X-axis magnetometer output
+  static constexpr uint8_t YMAGN_OUT = 0x12;  // Y-axis magnetometer output
+  static constexpr uint8_t ZMAGN_OUT = 0x14;  // Z-axis magnetometer output
+  static constexpr uint8_t BARO_OUT =
+      0x16;  // Barometer pressure measurement, high word
+  static constexpr uint8_t TEMP_OUT = 0x18;  // Temperature output
+  static constexpr uint8_t XGYRO_OFF =
+      0x1A;  // X-axis gyroscope bias offset factor
+  static constexpr uint8_t YGYRO_OFF =
+      0x1C;  // Y-axis gyroscope bias offset factor
+  static constexpr uint8_t ZGYRO_OFF =
+      0x1E;  // Z-axis gyroscope bias offset factor
+  static constexpr uint8_t XACCL_OFF =
+      0x20;  // X-axis acceleration bias offset factor
+  static constexpr uint8_t YACCL_OFF =
+      0x22;  // Y-axis acceleration bias offset factor
+  static constexpr uint8_t ZACCL_OFF =
+      0x24;  // Z-axis acceleration bias offset factor
+  static constexpr uint8_t XMAGN_HIC =
+      0x26;  // X-axis magnetometer, hard iron factor
+  static constexpr uint8_t YMAGN_HIC =
+      0x28;  // Y-axis magnetometer, hard iron factor
+  static constexpr uint8_t ZMAGN_HIC =
+      0x2A;  // Z-axis magnetometer, hard iron factor
+  static constexpr uint8_t XMAGN_SIC =
+      0x2C;  // X-axis magnetometer, soft iron factor
+  static constexpr uint8_t YMAGN_SIC =
+      0x2E;  // Y-axis magnetometer, soft iron factor
+  static constexpr uint8_t ZMAGN_SIC =
+      0x30;  // Z-axis magnetometer, soft iron factor
+  static constexpr uint8_t GPIO_CTRL = 0x32;  // GPIO control
+  static constexpr uint8_t MSC_CTRL = 0x34;   // MISC control
+  static constexpr uint8_t SMPL_PRD =
+      0x36;  // Sample clock/Decimation filter control
+  static constexpr uint8_t SENS_AVG = 0x38;    // Digital filter control
+  static constexpr uint8_t SEQ_CNT = 0x3A;     // MAGN_OUT and BARO_OUT counter
+  static constexpr uint8_t DIAG_STAT = 0x3C;   // System status
+  static constexpr uint8_t GLOB_CMD = 0x3E;    // System command
+  static constexpr uint8_t ALM_MAG1 = 0x40;    // Alarm 1 amplitude threshold
+  static constexpr uint8_t ALM_MAG2 = 0x42;    // Alarm 2 amplitude threshold
+  static constexpr uint8_t ALM_SMPL1 = 0x44;   // Alarm 1 sample size
+  static constexpr uint8_t ALM_SMPL2 = 0x46;   // Alarm 2 sample size
+  static constexpr uint8_t ALM_CTRL = 0x48;    // Alarm control
+  static constexpr uint8_t LOT_ID1 = 0x52;     // Lot identification number
+  static constexpr uint8_t LOT_ID2 = 0x54;     // Lot identification number
+  static constexpr uint8_t PROD_ID = 0x56;     // Product identifier
+  static constexpr uint8_t SERIAL_NUM = 0x58;  // Lot-specific serial number
+
+  /** @brief ADIS16448 Static Constants */
+  static constexpr double rad_to_deg = 57.2957795;
+  static constexpr double deg_to_rad = 0.0174532;
+  static constexpr double grav = 9.81;
+
+  /** @brief struct to store offset data */
+  struct OffsetData {
+    double gyro_rate_x = 0.0;
+    double gyro_rate_y = 0.0;
+    double gyro_rate_z = 0.0;
+  };
+
+  bool SwitchToStandardSPI();
+
+  bool SwitchToAutoSPI();
+
+  uint16_t ReadRegister(uint8_t reg);
+
+  void WriteRegister(uint8_t reg, uint16_t val);
+
+  void Acquire();
+
+  void Close();
+
+  // User-specified yaw axis
+  IMUAxis m_yaw_axis;
+
+  // Last read values (post-scaling)
+  double m_gyro_rate_x = 0.0;
+  double m_gyro_rate_y = 0.0;
+  double m_gyro_rate_z = 0.0;
+  double m_accel_x = 0.0;
+  double m_accel_y = 0.0;
+  double m_accel_z = 0.0;
+  double m_mag_x = 0.0;
+  double m_mag_y = 0.0;
+  double m_mag_z = 0.0;
+  double m_baro = 0.0;
+  double m_temp = 0.0;
+
+  // Complementary filter variables
+  double m_tau = 0.5;
+  double m_dt, m_alpha = 0.0;
+  double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
+
+  // vector for storing most recent imu values
+  OffsetData* m_offset_buffer = nullptr;
+
+  double m_gyro_rate_offset_x = 0.0;
+  double m_gyro_rate_offset_y = 0.0;
+  double m_gyro_rate_offset_z = 0.0;
+
+  // function to re-init offset buffer
+  void InitOffsetBuffer(int size);
+
+  // Accumulated gyro values (for offset calculation)
+  int m_avg_size = 0;
+  int m_accum_count = 0;
+
+  // Integrated gyro values
+  double m_integ_gyro_angle_x = 0.0;
+  double m_integ_gyro_angle_y = 0.0;
+  double m_integ_gyro_angle_z = 0.0;
+
+  // Complementary filter functions
+  double FormatFastConverge(double compAngle, double accAngle);
+
+  double FormatRange0to2PI(double compAngle);
+
+  double FormatAccelRange(double accelAngle, double accelZ);
+
+  double CompFilterProcess(double compAngle, double accelAngle, double omega);
+
+  // State and resource variables
+  volatile bool m_thread_active = false;
+  volatile bool m_first_run = true;
+  volatile bool m_thread_idle = false;
+  volatile bool m_start_up_mode = true;
+
+  bool m_auto_configured = false;
+  SPI::Port m_spi_port;
+  CalibrationTime m_calibration_time;
+  SPI* m_spi = nullptr;
+  DigitalInput* m_auto_interrupt = nullptr;
+
+  std::thread m_acquire_task;
+
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simGyroAngleX;
+  hal::SimDouble m_simGyroAngleY;
+  hal::SimDouble m_simGyroAngleZ;
+  hal::SimDouble m_simGyroRateX;
+  hal::SimDouble m_simGyroRateY;
+  hal::SimDouble m_simGyroRateZ;
+  hal::SimDouble m_simAccelX;
+  hal::SimDouble m_simAccelY;
+  hal::SimDouble m_simAccelZ;
+
+  struct NonMovableMutexWrapper {
+    wpi::mutex mutex;
+    NonMovableMutexWrapper() = default;
+
+    NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
+    NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
+
+    NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
+    NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
+      return *this;
+    }
+
+    void lock() { mutex.lock(); }
+
+    void unlock() { mutex.unlock(); }
+
+    bool try_lock() noexcept { return mutex.try_lock(); }
+  };
+
+  mutable NonMovableMutexWrapper m_mutex;
+
+  // CRC-16 Look-Up Table
+  static constexpr uint16_t adiscrc[256] = {
+      0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF, 0x1F3F,
+      0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890, 0x1E3D, 0x09F3,
+      0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992, 0x0102, 0x16CC, 0x0EDD,
+      0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD, 0x1C39, 0x0BF7, 0x13E6, 0x0428,
+      0x0387, 0x1449, 0x0C58, 0x1B96, 0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8,
+      0x0B76, 0x1367, 0x04A9, 0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74,
+      0x1265, 0x05AB, 0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A,
+      0x1A94, 0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
+      0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1, 0x060C,
+      0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3, 0x1933, 0x0EFD,
+      0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C, 0x0408, 0x13C6, 0x0BD7,
+      0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7, 0x1B37, 0x0CF9, 0x14E8, 0x0326,
+      0x0489, 0x1347, 0x0B56, 0x1C98, 0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B,
+      0x1245, 0x0A54, 0x1D9A, 0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A,
+      0x156B, 0x02A5, 0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040,
+      0x178E, 0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
+      0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3, 0x1123,
+      0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C, 0x0C18, 0x1BD6,
+      0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7, 0x1327, 0x04E9, 0x1CF8,
+      0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488, 0x1225, 0x05EB, 0x1DFA, 0x0A34,
+      0x0D9B, 0x1A55, 0x0244, 0x158A, 0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4,
+      0x056A, 0x1D7B, 0x0AB5, 0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060,
+      0x1871, 0x0FBF, 0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E,
+      0x1080, 0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
+      0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD, 0x1429,
+      0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386, 0x0B16, 0x1CD8,
+      0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9, 0x0A14, 0x1DDA, 0x05CB,
+      0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB, 0x152B, 0x02E5, 0x1AF4, 0x0D3A,
+      0x0A95, 0x1D5B, 0x054A, 0x1284};
+};
+
+}  // namespace frc

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

@@ -0,0 +1,417 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. All Rights Reserved.           */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*                                                                            */
+/* Juan Chong - frcsupport@analog.com                                         */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+#include <atomic>
+#include <memory>
+#include <thread>
+
+#include <hal/SimDevice.h>
+#include <networktables/NTSendable.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+#include "frc/DigitalSource.h"
+#include "frc/SPI.h"
+
+namespace frc {
+/**
+ * Use DMA SPI to read rate and acceleration data from the ADIS16470 IMU and
+ * return the robot's heading relative to a starting position and instant
+ * measurements
+ *
+ * The ADIS16470 gyro angle outputs track the robot's heading based on the
+ * starting position. As the robot rotates the new heading is computed by
+ * integrating the rate of rotation returned by the IMU. When the class is
+ * instantiated, a short calibration routine is performed where the IMU samples
+ * the gyros while at rest to determine the initial offset. This is subtracted
+ * from each sample to determine the heading.
+ *
+ * This class is for the ADIS16470 IMU connected via the primary SPI port
+ * available on the RoboRIO.
+ */
+
+class ADIS16470_IMU : public nt::NTSendable,
+                      public wpi::SendableHelper<ADIS16470_IMU> {
+ public:
+  /* ADIS16470 Calibration Time Enum Class */
+  enum class CalibrationTime {
+    _32ms = 0,
+    _64ms = 1,
+    _128ms = 2,
+    _256ms = 3,
+    _512ms = 4,
+    _1s = 5,
+    _2s = 6,
+    _4s = 7,
+    _8s = 8,
+    _16s = 9,
+    _32s = 10,
+    _64s = 11
+  };
+
+  enum IMUAxis { kX, kY, kZ };
+
+  /**
+   * @brief Default constructor. Uses CS0 on the 10-pin SPI port, the yaw axis
+   * is set to the IMU Z axis, and calibration time is defaulted to 4 seconds.
+   */
+  ADIS16470_IMU();
+
+  /**
+   * @brief Customizable constructor. Allows the SPI port and CS to be
+   * customized, the yaw axis used for GetAngle() is adjustable, and initial
+   * calibration time can be modified.
+   *
+   * @param yaw_axis Selects the "default" axis to use for GetAngle() and
+   * GetRate()
+   *
+   * @param port The SPI port and CS where the IMU is connected.
+   *
+   * @param cal_time The calibration time that should be used on start-up.
+   */
+  explicit ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
+                         CalibrationTime cal_time);
+
+  /**
+   * @brief Destructor. Kills the acquisiton loop and closes the SPI peripheral.
+   */
+  ~ADIS16470_IMU() override;
+
+  ADIS16470_IMU(ADIS16470_IMU&&) = default;
+  ADIS16470_IMU& operator=(ADIS16470_IMU&&) = default;
+
+  int ConfigDecRate(uint16_t reg);
+
+  /**
+   * @brief Switches the active SPI port to standard SPI mode, writes the
+   * command to activate the new null configuration, and re-enables auto SPI.
+   */
+  void Calibrate();
+
+  /**
+   * @brief Switches the active SPI port to standard SPI mode, writes a new
+   * value to the NULL_CNFG register in the IMU, and re-enables auto SPI.
+   */
+  int ConfigCalTime(CalibrationTime new_cal_time);
+
+  /**
+   * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
+   *
+   * Resets the gyro accumulations to a heading of zero. This can be used if
+   * the "zero" orientation of the sensor needs to be changed in runtime.
+   */
+  void Reset();
+
+  /**
+   * Returns the yaw axis angle in degrees (CCW positive).
+   */
+  units::degree_t GetAngle() const;
+
+  /**
+   * Returns the yaw axis angular rate in degrees per second (CCW positive).
+   */
+  units::degrees_per_second_t GetRate() const;
+
+  /**
+   * Returns the acceleration in the X axis.
+   */
+  units::meters_per_second_squared_t GetAccelX() const;
+
+  /**
+   * Returns the acceleration in the Y axis.
+   */
+  units::meters_per_second_squared_t GetAccelY() const;
+
+  /**
+   * Returns the acceleration in the Z axis.
+   */
+  units::meters_per_second_squared_t GetAccelZ() const;
+
+  units::degree_t GetXComplementaryAngle() const;
+
+  units::degree_t GetYComplementaryAngle() const;
+
+  units::degree_t GetXFilteredAccelAngle() const;
+
+  units::degree_t GetYFilteredAccelAngle() const;
+
+  IMUAxis GetYawAxis() const;
+
+  int SetYawAxis(IMUAxis yaw_axis);
+
+  // IMU yaw axis
+  IMUAxis m_yaw_axis;
+
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  int GetPort() const;
+
+  void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+  /* ADIS16470 Register Map Declaration */
+  static constexpr uint8_t FLASH_CNT = 0x00;  // Flash memory write count
+  static constexpr uint8_t DIAG_STAT =
+      0x02;  // Diagnostic and operational status
+  static constexpr uint8_t X_GYRO_LOW =
+      0x04;  // X-axis gyroscope output, lower word
+  static constexpr uint8_t X_GYRO_OUT =
+      0x06;  // X-axis gyroscope output, upper word
+  static constexpr uint8_t Y_GYRO_LOW =
+      0x08;  // Y-axis gyroscope output, lower word
+  static constexpr uint8_t Y_GYRO_OUT =
+      0x0A;  // Y-axis gyroscope output, upper word
+  static constexpr uint8_t Z_GYRO_LOW =
+      0x0C;  // Z-axis gyroscope output, lower word
+  static constexpr uint8_t Z_GYRO_OUT =
+      0x0E;  // Z-axis gyroscope output, upper word
+  static constexpr uint8_t X_ACCL_LOW =
+      0x10;  // X-axis accelerometer output, lower word
+  static constexpr uint8_t X_ACCL_OUT =
+      0x12;  // X-axis accelerometer output, upper word
+  static constexpr uint8_t Y_ACCL_LOW =
+      0x14;  // Y-axis accelerometer output, lower word
+  static constexpr uint8_t Y_ACCL_OUT =
+      0x16;  // Y-axis accelerometer output, upper word
+  static constexpr uint8_t Z_ACCL_LOW =
+      0x18;  // Z-axis accelerometer output, lower word
+  static constexpr uint8_t Z_ACCL_OUT =
+      0x1A;  // Z-axis accelerometer output, upper word
+  static constexpr uint8_t TEMP_OUT =
+      0x1C;  // Temperature output (internal, not calibrated)
+  static constexpr uint8_t TIME_STAMP = 0x1E;  // PPS mode time stamp
+  static constexpr uint8_t X_DELTANG_LOW =
+      0x24;  // X-axis delta angle output, lower word
+  static constexpr uint8_t X_DELTANG_OUT =
+      0x26;  // X-axis delta angle output, upper word
+  static constexpr uint8_t Y_DELTANG_LOW =
+      0x28;  // Y-axis delta angle output, lower word
+  static constexpr uint8_t Y_DELTANG_OUT =
+      0x2A;  // Y-axis delta angle output, upper word
+  static constexpr uint8_t Z_DELTANG_LOW =
+      0x2C;  // Z-axis delta angle output, lower word
+  static constexpr uint8_t Z_DELTANG_OUT =
+      0x2E;  // Z-axis delta angle output, upper word
+  static constexpr uint8_t X_DELTVEL_LOW =
+      0x30;  // X-axis delta velocity output, lower word
+  static constexpr uint8_t X_DELTVEL_OUT =
+      0x32;  // X-axis delta velocity output, upper word
+  static constexpr uint8_t Y_DELTVEL_LOW =
+      0x34;  // Y-axis delta velocity output, lower word
+  static constexpr uint8_t Y_DELTVEL_OUT =
+      0x36;  // Y-axis delta velocity output, upper word
+  static constexpr uint8_t Z_DELTVEL_LOW =
+      0x38;  // Z-axis delta velocity output, lower word
+  static constexpr uint8_t Z_DELTVEL_OUT =
+      0x3A;  // Z-axis delta velocity output, upper word
+  static constexpr uint8_t XG_BIAS_LOW =
+      0x40;  // X-axis gyroscope bias offset correction, lower word
+  static constexpr uint8_t XG_BIAS_HIGH =
+      0x42;  // X-axis gyroscope bias offset correction, upper word
+  static constexpr uint8_t YG_BIAS_LOW =
+      0x44;  // Y-axis gyroscope bias offset correction, lower word
+  static constexpr uint8_t YG_BIAS_HIGH =
+      0x46;  // Y-axis gyroscope bias offset correction, upper word
+  static constexpr uint8_t ZG_BIAS_LOW =
+      0x48;  // Z-axis gyroscope bias offset correction, lower word
+  static constexpr uint8_t ZG_BIAS_HIGH =
+      0x4A;  // Z-axis gyroscope bias offset correction, upper word
+  static constexpr uint8_t XA_BIAS_LOW =
+      0x4C;  // X-axis accelerometer bias offset correction, lower word
+  static constexpr uint8_t XA_BIAS_HIGH =
+      0x4E;  // X-axis accelerometer bias offset correction, upper word
+  static constexpr uint8_t YA_BIAS_LOW =
+      0x50;  // Y-axis accelerometer bias offset correction, lower word
+  static constexpr uint8_t YA_BIAS_HIGH =
+      0x52;  // Y-axis accelerometer bias offset correction, upper word
+  static constexpr uint8_t ZA_BIAS_LOW =
+      0x54;  // Z-axis accelerometer bias offset correction, lower word
+  static constexpr uint8_t ZA_BIAS_HIGH =
+      0x56;  // Z-axis accelerometer bias offset correction, upper word
+  static constexpr uint8_t FILT_CTRL = 0x5C;  // Filter control
+  static constexpr uint8_t MSC_CTRL = 0x60;   // Miscellaneous control
+  static constexpr uint8_t UP_SCALE = 0x62;   // Clock scale factor, PPS mode
+  static constexpr uint8_t DEC_RATE =
+      0x64;  // Decimation rate control (output data rate)
+  static constexpr uint8_t NULL_CNFG = 0x66;  // Auto-null configuration control
+  static constexpr uint8_t GLOB_CMD = 0x68;   // Global commands
+  static constexpr uint8_t FIRM_REV = 0x6C;   // Firmware revision
+  static constexpr uint8_t FIRM_DM =
+      0x6E;  // Firmware revision date, month and day
+  static constexpr uint8_t FIRM_Y = 0x70;   // Firmware revision date, year
+  static constexpr uint8_t PROD_ID = 0x72;  // Product identification
+  static constexpr uint8_t SERIAL_NUM =
+      0x74;  // Serial number (relative to assembly lot)
+  static constexpr uint8_t USER_SCR1 = 0x76;  // User scratch register 1
+  static constexpr uint8_t USER_SCR2 = 0x78;  // User scratch register 2
+  static constexpr uint8_t USER_SCR3 = 0x7A;  // User scratch register 3
+  static constexpr uint8_t FLSHCNT_LOW =
+      0x7C;  // Flash update count, lower word
+  static constexpr uint8_t FLSHCNT_HIGH =
+      0x7E;  // Flash update count, upper word
+
+  /* ADIS16470 Auto SPI Data Packets */
+  static constexpr uint8_t m_autospi_x_packet[16] = {
+      X_DELTANG_OUT, FLASH_CNT, X_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+      Y_GYRO_OUT,    FLASH_CNT, Z_GYRO_OUT,    FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+      Y_ACCL_OUT,    FLASH_CNT, Z_ACCL_OUT,    FLASH_CNT};
+
+  static constexpr uint8_t m_autospi_y_packet[16] = {
+      Y_DELTANG_OUT, FLASH_CNT, Y_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+      Y_GYRO_OUT,    FLASH_CNT, Z_GYRO_OUT,    FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+      Y_ACCL_OUT,    FLASH_CNT, Z_ACCL_OUT,    FLASH_CNT};
+
+  static constexpr uint8_t m_autospi_z_packet[16] = {
+      Z_DELTANG_OUT, FLASH_CNT, Z_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+      Y_GYRO_OUT,    FLASH_CNT, Z_GYRO_OUT,    FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+      Y_ACCL_OUT,    FLASH_CNT, Z_ACCL_OUT,    FLASH_CNT};
+
+  /* ADIS16470 Constants */
+  static constexpr double delta_angle_sf =
+      2160.0 / 2147483648.0; /* 2160 / (2^31) */
+  static constexpr double rad_to_deg = 57.2957795;
+  static constexpr double deg_to_rad = 0.0174532;
+  static constexpr double grav = 9.81;
+
+  /**
+   * @brief Switches to standard SPI operation. Primarily used when exiting auto
+   * SPI mode.
+   *
+   * @return A boolean indicating the success or failure of setting up the SPI
+   * peripheral in standard SPI mode.
+   */
+  bool SwitchToStandardSPI();
+
+  /**
+   * @brief Switches to auto SPI operation. Primarily used when exiting standard
+   * SPI mode.
+   *
+   * @return A boolean indicating the success or failure of setting up the SPI
+   * peripheral in auto SPI mode.
+   */
+  bool SwitchToAutoSPI();
+
+  /**
+   * @brief Reads the contents of a specified register location over SPI.
+   *
+   * @param reg An unsigned, 8-bit register location.
+   *
+   * @return An unsigned, 16-bit number representing the contents of the
+   * requested register location.
+   */
+  uint16_t ReadRegister(uint8_t reg);
+
+  /**
+   * @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
+   * locations over SPI.
+   *
+   * @param reg An unsigned, 8-bit register location.
+   *
+   * @param val An unsigned, 16-bit value to be written to the specified
+   * register location.
+   */
+  void WriteRegister(uint8_t reg, uint16_t val);
+
+  /**
+   * @brief Main acquisition loop. Typically called asynchronously and
+   * free-wheels while the robot code is active.
+   */
+  void Acquire();
+
+  void Close();
+
+  // Integrated gyro value
+  double m_integ_angle = 0.0;
+
+  // Instant raw outputs
+  double m_gyro_rate_x = 0.0;
+  double m_gyro_rate_y = 0.0;
+  double m_gyro_rate_z = 0.0;
+  double m_accel_x = 0.0;
+  double m_accel_y = 0.0;
+  double m_accel_z = 0.0;
+
+  // Complementary filter variables
+  double m_tau = 1.0;
+  double m_dt, m_alpha = 0.0;
+  double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
+
+  // Complementary filter functions
+  double FormatFastConverge(double compAngle, double accAngle);
+
+  double FormatRange0to2PI(double compAngle);
+
+  double FormatAccelRange(double accelAngle, double accelZ);
+
+  double CompFilterProcess(double compAngle, double accelAngle, double omega);
+
+  // State and resource variables
+  volatile bool m_thread_active = false;
+  volatile bool m_first_run = true;
+  volatile bool m_thread_idle = false;
+  bool m_auto_configured = false;
+  SPI::Port m_spi_port;
+  uint16_t m_calibration_time;
+  SPI* m_spi = nullptr;
+  DigitalInput* m_auto_interrupt = nullptr;
+  double m_scaled_sample_rate = 2500.0;  // Default sample rate setting
+
+  std::thread m_acquire_task;
+
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simGyroAngleX;
+  hal::SimDouble m_simGyroAngleY;
+  hal::SimDouble m_simGyroAngleZ;
+  hal::SimDouble m_simGyroRateX;
+  hal::SimDouble m_simGyroRateY;
+  hal::SimDouble m_simGyroRateZ;
+  hal::SimDouble m_simAccelX;
+  hal::SimDouble m_simAccelY;
+  hal::SimDouble m_simAccelZ;
+
+  struct NonMovableMutexWrapper {
+    wpi::mutex mutex;
+    NonMovableMutexWrapper() = default;
+
+    NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
+    NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
+
+    NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
+    NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
+      return *this;
+    }
+
+    void lock() { mutex.lock(); }
+
+    void unlock() { mutex.unlock(); }
+
+    bool try_lock() noexcept { return mutex.try_lock(); }
+  };
+
+  mutable NonMovableMutexWrapper m_mutex;
+};
+
+}  // namespace frc

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,106 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+namespace frc {
+
+class ADIS16448_IMU;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADIS16448 IMU.
+ */
+class ADIS16448_IMUSim {
+ public:
+  /**
+   * Constructs from a ADIS16448_IMU object.
+   *
+   * @param imu ADIS16448_IMU to simulate
+   */
+  explicit ADIS16448_IMUSim(const ADIS16448_IMU& imu);
+
+  /**
+   * Sets the X axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleX(units::degree_t angle);
+
+  /**
+   * Sets the Y axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleY(units::degree_t angle);
+
+  /**
+   * Sets the Z axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleZ(units::degree_t angle);
+
+  /**
+   * Sets the X axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateX(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the Y axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateY(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the Z axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateZ(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the X axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelX(units::meters_per_second_squared_t accel);
+
+  /**
+   * Sets the Y axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelY(units::meters_per_second_squared_t accel);
+
+  /**
+   * Sets the Z axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelZ(units::meters_per_second_squared_t accel);
+
+ private:
+  hal::SimDouble m_simGyroAngleX;
+  hal::SimDouble m_simGyroAngleY;
+  hal::SimDouble m_simGyroAngleZ;
+  hal::SimDouble m_simGyroRateX;
+  hal::SimDouble m_simGyroRateY;
+  hal::SimDouble m_simGyroRateZ;
+  hal::SimDouble m_simAccelX;
+  hal::SimDouble m_simAccelY;
+  hal::SimDouble m_simAccelZ;
+};
+
+}  // namespace sim
+}  // namespace frc

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

@@ -0,0 +1,106 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+namespace frc {
+
+class ADIS16470_IMU;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADIS16470 IMU.
+ */
+class ADIS16470_IMUSim {
+ public:
+  /**
+   * Constructs from a ADIS16470_IMU object.
+   *
+   * @param imu ADIS16470_IMU to simulate
+   */
+  explicit ADIS16470_IMUSim(const ADIS16470_IMU& imu);
+
+  /**
+   * Sets the X axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleX(units::degree_t angle);
+
+  /**
+   * Sets the Y axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleY(units::degree_t angle);
+
+  /**
+   * Sets the Z axis angle (CCW positive).
+   *
+   * @param angle The angle.
+   */
+  void SetGyroAngleZ(units::degree_t angle);
+
+  /**
+   * Sets the X axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateX(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the Y axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateY(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the Z axis angular rate (CCW positive).
+   *
+   * @param angularRate The angular rate.
+   */
+  void SetGyroRateZ(units::degrees_per_second_t angularRate);
+
+  /**
+   * Sets the X axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelX(units::meters_per_second_squared_t accel);
+
+  /**
+   * Sets the Y axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelY(units::meters_per_second_squared_t accel);
+
+  /**
+   * Sets the Z axis acceleration.
+   *
+   * @param accel The acceleration.
+   */
+  void SetAccelZ(units::meters_per_second_squared_t accel);
+
+ private:
+  hal::SimDouble m_simGyroAngleX;
+  hal::SimDouble m_simGyroAngleY;
+  hal::SimDouble m_simGyroAngleZ;
+  hal::SimDouble m_simGyroRateX;
+  hal::SimDouble m_simGyroRateY;
+  hal::SimDouble m_simGyroRateZ;
+  hal::SimDouble m_simAccelX;
+  hal::SimDouble m_simAccelY;
+  hal::SimDouble m_simAccelZ;
+};
+
+}  // namespace sim
+}  // namespace frc

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/DigitalCommunication/cpp/Robot.cpp

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/DigitalOutput.h>
+#include <frc/DriverStation.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * This is a sample program demonstrating how to communicate to a light
+ * controller from the robot code using the roboRIO's DIO ports.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotPeriodic() override {
+    // pull alliance port high if on red alliance, pull low if on blue alliance
+    m_allianceOutput.Set(frc::DriverStation::GetAlliance() ==
+                         frc::DriverStation::kRed);
+
+    // pull enabled port high if enabled, low if disabled
+    m_enabledOutput.Set(frc::DriverStation::IsEnabled());
+
+    // pull auto port high if in autonomous, low if in teleop (or disabled)
+    m_autonomousOutput.Set(frc::DriverStation::IsAutonomous());
+
+    // pull alert port high if match time remaining is between 30 and 25 seconds
+    auto matchTime = frc::DriverStation::GetMatchTime();
+    m_alertOutput.Set(matchTime <= 30 && matchTime >= 25);
+  }
+
+ private:
+  // define ports for communication with light controller
+  static constexpr int kAlliancePort = 0;
+  static constexpr int kEnabledPort = 1;
+  static constexpr int kAutonomousPort = 2;
+  static constexpr int kAlertPort = 3;
+
+  frc::DigitalOutput m_allianceOutput{kAlliancePort};
+  frc::DigitalOutput m_enabledOutput{kEnabledPort};
+  frc::DigitalOutput m_autonomousOutput{kAutonomousPort};
+  frc::DigitalOutput m_alertOutput{kAlertPort};
+};
+
+int main() {
+  return frc::StartRobot<Robot>();
+}

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/I2CCommunication/cpp/Robot.cpp

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <fmt/format.h>
+
+#include <frc/DriverStation.h>
+#include <frc/I2C.h>
+#include <frc/TimedRobot.h>
+#include <frc/Timer.h>
+
+/**
+ * This is a sample program demonstrating how to communicate to a light
+ * controller from the robot code using the roboRIO's I2C port.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotPeriodic() override {
+    // Creates a string to hold current robot state information, including
+    // alliance, enabled state, operation mode, and match time. The message
+    // is sent in format "AEM###" where A is the alliance color, (R)ed or
+    // (B)lue, E is the enabled state, (E)nabled or (D)isabled, M is the
+    // operation mode, (A)utonomous or (T)eleop, and ### is the zero-padded
+    // time remaining in the match.
+    //
+    // For example, "RET043" would indicate that the robot is on the red
+    // alliance, enabled in teleop mode, with 43 seconds left in the match.
+    auto string = fmt::format(
+        "{}{}{}{:03}",
+        frc::DriverStation::GetAlliance() == frc::DriverStation::Alliance::kRed
+            ? "R"
+            : "B",
+        frc::DriverStation::IsEnabled() ? "E" : "D",
+        frc::DriverStation::IsAutonomous() ? "A" : "T",
+        static_cast<int>(frc::Timer::GetMatchTime().value()));
+
+    arduino.WriteBulk(reinterpret_cast<uint8_t*>(string.data()), string.size());
+  }
+
+ private:
+  static constexpr int deviceAddress = 4;
+  frc::I2C arduino{frc::I2C::Port::kOnboard, deviceAddress};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

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

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -260,6 +260,27 @@
     "gradlebase": "cpp",
     "commandversion": 2
   },
+  {
+    "name": "I2C Communication",
+    "description": "An example program that communicates with external devices (such as an Arduino) using the roboRIO's I2C port",
+    "tags": [
+      "I2C"
+    ],
+    "foldername": "I2CCommunication",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+  {
+    "name": "Digital Communication Sample",
+    "description": "An example program that communicates with external devices (such as an Arduino) using the roboRIO's DIO",
+    "tags": [
+      "Digital"
+    ],
+    "foldername": "DigitalCommunication",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+
   {
     "name": "Axis Camera Sample",
     "description": "An example program that acquires images from an Axis network camera and adds some annotation to the image as you might do for showing operators the result of some image recognition, and sends it to the dashboard for display. This demonstrates the use of the AxisCamera class.",

wpilibcExamples/src/main/cpp/templates/templates.json

@@ -1,4 +1,14 @@
 [
+  {
+    "name": "Command Robot",
+    "description": "Command style",
+    "tags": [
+      "Command"
+    ],
+    "foldername": "commandbased",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
   {
     "name": "Timed Robot",
     "description": "Timed style",
@@ -30,13 +40,13 @@
     "commandversion": 2
   },
   {
-    "name": "Command Robot",
-    "description": "Command style",
+    "name": "Romi - Command Robot",
+    "description": "Romi - Command style",
     "tags": [
-      "Command"
+      "Command", "Romi"
     ],
     "foldername": "commandbased",
-    "gradlebase": "cpp",
+    "gradlebase": "cppromi",
     "commandversion": 2
   },
   {
@@ -48,15 +58,5 @@
     "foldername": "timed",
     "gradlebase": "cppromi",
     "commandversion": 2
-  },
-  {
-    "name": "Romi - Command Robot",
-    "description": "Romi - Command style",
-    "tags": [
-      "Command", "Romi"
-    ],
-    "foldername": "commandbased",
-    "gradlebase": "cppromi",
-    "commandversion": 2
   }
 ]