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

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

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) {

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

@@ -81,6 +81,12 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
         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 =
@@ -237,7 +243,7 @@ void ADIS16448_IMU::InitOffsetBuffer(int size) {
   if (m_offset_buffer != nullptr) {
     delete[] m_offset_buffer;
   }
-  m_offset_buffer = new offset_data[size];
+  m_offset_buffer = new OffsetData[size];
 
   // set accumulate count to 0
   m_accum_count = 0;
@@ -324,23 +330,23 @@ void ADIS16448_IMU::Calibrate() {
   std::scoped_lock sync(m_mutex);
   // Calculate the running average
   int gyroAverageSize = std::min(m_accum_count, m_avg_size);
-  double m_gyro_accum_x = 0.0;
-  double m_gyro_accum_y = 0.0;
-  double m_gyro_accum_z = 0.0;
+  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++) {
-    m_gyro_accum_x += m_offset_buffer[i].m_accum_gyro_x;
-    m_gyro_accum_y += m_offset_buffer[i].m_accum_gyro_y;
-    m_gyro_accum_z += m_offset_buffer[i].m_accum_gyro_z;
+    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_offset_x = m_gyro_accum_x / gyroAverageSize;
-  m_gyro_offset_y = m_gyro_accum_y / gyroAverageSize;
-  m_gyro_offset_z = m_gyro_accum_z / gyroAverageSize;
-  m_integ_gyro_x = 0.0;
-  m_integ_gyro_y = 0.0;
-  m_integ_gyro_z = 0.0;
+  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_offset_x << " Y off: " << m_gyro_offset_y << " Z off: " <<
-  // m_gyro_offset_z << std::endl;
+  // m_gyro_rate_offset_x << " Y off: " << m_gyro_rate_offset_y << " Z off: " <<
+  // m_gyro_rate_offset_z << std::endl;
 }
 
 /**
@@ -384,9 +390,9 @@ void ADIS16448_IMU::WriteRegister(uint8_t reg, uint16_t val) {
  **/
 void ADIS16448_IMU::Reset() {
   std::scoped_lock sync(m_mutex);
-  m_integ_gyro_x = 0.0;
-  m_integ_gyro_y = 0.0;
-  m_integ_gyro_z = 0.0;
+  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() {
@@ -422,9 +428,6 @@ void ADIS16448_IMU::Acquire() {
 
   const int BUFFER_SIZE = 4000;
 
-  // struct to store accumulate data
-  offset_data sample_data;
-
   // This buffer can contain many datasets
   uint32_t buffer[BUFFER_SIZE];
   int data_count = 0;
@@ -432,9 +435,9 @@ void ADIS16448_IMU::Acquire() {
   int data_to_read = 0;
   int bufferAvgIndex = 0;
   uint32_t previous_timestamp = 0;
-  double gyro_x = 0.0;
-  double gyro_y = 0.0;
-  double gyro_z = 0.0;
+  double 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;
@@ -443,9 +446,9 @@ void ADIS16448_IMU::Acquire() {
   double mag_z = 0.0;
   double baro = 0.0;
   double temp = 0.0;
-  double gyro_x_si = 0.0;
-  double gyro_y_si = 0.0;
-  // double gyro_z_si = 0.0;
+  double 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;
@@ -502,9 +505,9 @@ void ADIS16448_IMU::Acquire() {
           // Timestamp is at buffer[i]
           m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
           // Split array and scale data
-          gyro_x = BuffToShort(&buffer[i + 5]) * 0.04;
-          gyro_y = BuffToShort(&buffer[i + 7]) * 0.04;
-          gyro_z = BuffToShort(&buffer[i + 9]) * 0.04;
+          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;
@@ -525,9 +528,9 @@ void ADIS16448_IMU::Acquire() {
           BuffToShort(&buffer[i + 27]) << std::endl; */
 
           // Convert scaled sensor data to SI units
-          gyro_x_si = gyro_x * deg_to_rad;
-          gyro_y_si = gyro_y * deg_to_rad;
-          // gyro_z_si = gyro_z * deg_to_rad;
+          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;
@@ -554,8 +557,10 @@ void ADIS16448_IMU::Acquire() {
                                          (-accel_z_si * -accel_z_si)));
             accelAngleX = FormatAccelRange(accelAngleX, -accel_z_si);
             accelAngleY = FormatAccelRange(accelAngleY, -accel_z_si);
-            compAngleX = CompFilterProcess(compAngleX, accelAngleX, -gyro_y_si);
-            compAngleY = CompFilterProcess(compAngleY, accelAngleY, -gyro_x_si);
+            compAngleX =
+                CompFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
+            compAngleY =
+                CompFilterProcess(compAngleY, accelAngleY, -gyro_rate_x_si);
           }
 
           // Update global variables and state
@@ -563,27 +568,24 @@ void ADIS16448_IMU::Acquire() {
             std::scoped_lock sync(m_mutex);
             // Ignore first, integrated sample
             if (m_first_run) {
-              m_integ_gyro_x = 0.0;
-              m_integ_gyro_y = 0.0;
-              m_integ_gyro_z = 0.0;
+              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
-              // Build most recent sample data
-              sample_data.m_accum_gyro_x = gyro_x;
-              sample_data.m_accum_gyro_y = gyro_y;
-              sample_data.m_accum_gyro_z = gyro_z;
-              // Add to buffer
+              // Add most recent sample data to buffer
               bufferAvgIndex = m_accum_count % m_avg_size;
-              m_offset_buffer[bufferAvgIndex] = sample_data;
+              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_x = gyro_x;
-              m_gyro_y = gyro_y;
-              m_gyro_z = gyro_z;
+              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;
@@ -598,9 +600,12 @@ void ADIS16448_IMU::Acquire() {
               m_accelAngleY = accelAngleY * rad_to_deg;
               // Accumulate gyro for angle integration and publish to global
               // variables
-              m_integ_gyro_x += (gyro_x - m_gyro_offset_x) * m_dt;
-              m_integ_gyro_y += (gyro_y - m_gyro_offset_y) * m_dt;
-              m_integ_gyro_z += (gyro_z - m_gyro_offset_z) * m_dt;
+              m_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;
@@ -628,9 +633,9 @@ void ADIS16448_IMU::Acquire() {
       data_remainder = 0;
       data_to_read = 0;
       previous_timestamp = 0.0;
-      gyro_x = 0.0;
-      gyro_y = 0.0;
-      gyro_z = 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;
@@ -639,9 +644,9 @@ void ADIS16448_IMU::Acquire() {
       mag_z = 0.0;
       baro = 0.0;
       temp = 0.0;
-      gyro_x_si = 0.0;
-      gyro_y_si = 0.0;
-      // gyro_z_si = 0.0;
+      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;
@@ -742,12 +747,25 @@ units::degree_t ADIS16448_IMU::GetAngle() const {
   }
 }
 
+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_x};
+  return units::degree_t{m_integ_gyro_angle_x};
 }
 
 units::degree_t ADIS16448_IMU::GetGyroAngleY() const {
@@ -755,7 +773,7 @@ units::degree_t ADIS16448_IMU::GetGyroAngleY() const {
     return units::degree_t{m_simGyroAngleY.Get()};
   }
   std::scoped_lock sync(m_mutex);
-  return units::degree_t{m_integ_gyro_y};
+  return units::degree_t{m_integ_gyro_angle_y};
 }
 
 units::degree_t ADIS16448_IMU::GetGyroAngleZ() const {
@@ -763,7 +781,31 @@ units::degree_t ADIS16448_IMU::GetGyroAngleZ() const {
     return units::degree_t{m_simGyroAngleZ.Get()};
   }
   std::scoped_lock sync(m_mutex);
-  return units::degree_t{m_integ_gyro_z};
+  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 {

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

@@ -79,6 +79,12 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
         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 =
@@ -499,15 +505,15 @@ void ADIS16470_IMU::Acquire() {
   int data_to_read = 0;
   uint32_t previous_timestamp = 0;
   double delta_angle = 0.0;
-  double gyro_x = 0.0;
-  double gyro_y = 0.0;
-  double gyro_z = 0.0;
+  double 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_x_si = 0.0;
-  double gyro_y_si = 0.0;
-  // double gyro_z_si = 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;
@@ -561,17 +567,17 @@ void ADIS16470_IMU::Acquire() {
          * time (based on timestamp) */
         delta_angle = (ToInt(&buffer[i + 3]) * delta_angle_sf) /
                       (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
-        gyro_x = (BuffToShort(&buffer[i + 7]) / 10.0);
-        gyro_y = (BuffToShort(&buffer[i + 9]) / 10.0);
-        gyro_z = (BuffToShort(&buffer[i + 11]) / 10.0);
+        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_x_si = gyro_x * deg_to_rad;
-        gyro_y_si = gyro_y * deg_to_rad;
-        // gyro_z_si = gyro_z * deg_to_rad;
+        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;
@@ -602,8 +608,10 @@ void ADIS16470_IMU::Acquire() {
                                                  (accel_z_si * accel_z_si)));
           accelAngleX = FormatAccelRange(accelAngleX, accel_z_si);
           accelAngleY = FormatAccelRange(accelAngleY, accel_z_si);
-          compAngleX = CompFilterProcess(compAngleX, accelAngleX, -gyro_y_si);
-          compAngleY = CompFilterProcess(compAngleY, accelAngleY, gyro_x_si);
+          compAngleX =
+              CompFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
+          compAngleY =
+              CompFilterProcess(compAngleY, accelAngleY, gyro_rate_x_si);
         }
 
         // DEBUG: Print accumulated values
@@ -619,9 +627,9 @@ void ADIS16470_IMU::Acquire() {
           } else {
             m_integ_angle += delta_angle;
           }
-          m_gyro_x = gyro_x;
-          m_gyro_y = gyro_y;
-          m_gyro_z = gyro_z;
+          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;
@@ -639,15 +647,15 @@ void ADIS16470_IMU::Acquire() {
       data_to_read = 0;
       previous_timestamp = 0.0;
       delta_angle = 0.0;
-      gyro_x = 0.0;
-      gyro_y = 0.0;
-      gyro_z = 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_x_si = 0.0;
-      gyro_y_si = 0.0;
-      // gyro_z_si = 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;
@@ -700,17 +708,6 @@ double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
   return compAngle;
 }
 
-/**
- * @brief Returns the current integrated angle for the axis specified.
- *
- * @param m_yaw_axis An enum indicating the axis chosen to act as the yaw axis.
- *
- * @return The current integrated angle in degrees.
- *
- * This function returns the most recent integrated angle for the axis chosen by
- *m_yaw_axis. This function is most useful in situations where the yaw axis may
- *not coincide with the IMU Z axis.
- **/
 units::degree_t ADIS16470_IMU::GetAngle() const {
   switch (m_yaw_axis) {
     case kX:
@@ -733,6 +730,30 @@ units::degree_t ADIS16470_IMU::GetAngle() const {
   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()};

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

@@ -14,6 +14,9 @@ ADIS16448_IMUSim::ADIS16448_IMUSim(const frc::ADIS16448_IMU& imu) {
   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");
@@ -31,6 +34,18 @@ 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());
 }

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

@@ -14,6 +14,9 @@ ADIS16470_IMUSim::ADIS16470_IMUSim(const frc::ADIS16470_IMU& imu) {
   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");
@@ -31,6 +34,18 @@ 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());
 }

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

@@ -133,6 +133,11 @@ class ADIS16448_IMU : public nt::NTSendable,
    */
   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.
    */
@@ -148,6 +153,21 @@ class ADIS16448_IMU : public nt::NTSendable,
    */
   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.
    */
@@ -259,10 +279,10 @@ class ADIS16448_IMU : public nt::NTSendable,
   static constexpr double grav = 9.81;
 
   /** @brief struct to store offset data */
-  struct offset_data {
-    double m_accum_gyro_x = 0.0;
-    double m_accum_gyro_y = 0.0;
-    double m_accum_gyro_z = 0.0;
+  struct OffsetData {
+    double gyro_rate_x = 0.0;
+    double gyro_rate_y = 0.0;
+    double gyro_rate_z = 0.0;
   };
 
   bool SwitchToStandardSPI();
@@ -281,9 +301,9 @@ class ADIS16448_IMU : public nt::NTSendable,
   IMUAxis m_yaw_axis;
 
   // Last read values (post-scaling)
-  double m_gyro_x = 0.0;
-  double m_gyro_y = 0.0;
-  double m_gyro_z = 0.0;
+  double m_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;
@@ -299,11 +319,11 @@ class ADIS16448_IMU : public nt::NTSendable,
   double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
 
   // vector for storing most recent imu values
-  offset_data* m_offset_buffer = nullptr;
+  OffsetData* m_offset_buffer = nullptr;
 
-  double m_gyro_offset_x = 0.0;
-  double m_gyro_offset_y = 0.0;
-  double m_gyro_offset_z = 0.0;
+  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);
@@ -313,9 +333,9 @@ class ADIS16448_IMU : public nt::NTSendable,
   int m_accum_count = 0;
 
   // Integrated gyro values
-  double m_integ_gyro_x = 0.0;
-  double m_integ_gyro_y = 0.0;
-  double m_integ_gyro_z = 0.0;
+  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);
@@ -344,6 +364,9 @@ class ADIS16448_IMU : public nt::NTSendable,
   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;

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

@@ -127,6 +127,11 @@ class ADIS16470_IMU : public nt::NTSendable,
    */
   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.
    */
@@ -342,7 +347,12 @@ class ADIS16470_IMU : public nt::NTSendable,
   double m_integ_angle = 0.0;
 
   // Instant raw outputs
-  double m_gyro_x, m_gyro_y, m_gyro_z, m_accel_x, m_accel_y, m_accel_z = 0.0;
+  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;
@@ -375,6 +385,9 @@ class ADIS16470_IMU : public nt::NTSendable,
   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;

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

@@ -48,6 +48,27 @@ class ADIS16448_IMUSim {
    */
   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.
    *
@@ -73,6 +94,9 @@ class ADIS16448_IMUSim {
   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;

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

@@ -48,6 +48,27 @@ class ADIS16470_IMUSim {
    */
   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.
    *
@@ -73,6 +94,9 @@ class ADIS16470_IMUSim {
   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;

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/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/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
   }
 ]

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

@@ -129,14 +129,14 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
   private IMUAxis m_yaw_axis;
 
   /* Offset data storage */
-  private double m_accum_gyro_x[];
-  private double m_accum_gyro_y[];
-  private double m_accum_gyro_z[];
+  private double m_offset_data_gyro_rate_x[];
+  private double m_offset_data_gyro_rate_y[];
+  private double m_offset_data_gyro_rate_z[];
 
   /* Instant raw output variables */
-  private double m_gyro_x = 0.0;
-  private double m_gyro_y = 0.0;
-  private double m_gyro_z = 0.0;
+  private double m_gyro_rate_x = 0.0;
+  private double m_gyro_rate_y = 0.0;
+  private double m_gyro_rate_z = 0.0;
   private double m_accel_x = 0.0;
   private double m_accel_y = 0.0;
   private double m_accel_z = 0.0;
@@ -147,16 +147,16 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
   private double m_temp = 0.0;
 
   /* IMU gyro offset variables */
-  private double m_gyro_offset_x = 0.0;
-  private double m_gyro_offset_y = 0.0;
-  private double m_gyro_offset_z = 0.0;
+  private double m_gyro_rate_offset_x = 0.0;
+  private double m_gyro_rate_offset_y = 0.0;
+  private double m_gyro_rate_offset_z = 0.0;
   private int m_avg_size = 0;
   private int m_accum_count = 0;
 
   /* Integrated gyro angle variables */
-  private double m_integ_gyro_x = 0.0;
-  private double m_integ_gyro_y = 0.0;
-  private double m_integ_gyro_z = 0.0;
+  private double m_integ_gyro_angle_x = 0.0;
+  private double m_integ_gyro_angle_y = 0.0;
+  private double m_integ_gyro_angle_z = 0.0;
 
   /* Complementary filter variables */
   private double m_dt = 0.0;
@@ -188,6 +188,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
   private SimDouble m_simGyroAngleX;
   private SimDouble m_simGyroAngleY;
   private SimDouble m_simGyroAngleZ;
+  private SimDouble m_simGyroRateX;
+  private SimDouble m_simGyroRateY;
+  private SimDouble m_simGyroRateZ;
   private SimDouble m_simAccelX;
   private SimDouble m_simAccelY;
   private SimDouble m_simAccelZ;
@@ -262,6 +265,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
       m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0);
       m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0);
       m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0);
       m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0);
       m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0);
       m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0);
@@ -532,9 +538,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
     m_avg_size = size;
     synchronized (this) {
       // Resize vector
-      m_accum_gyro_x = new double[size];
-      m_accum_gyro_y = new double[size];
-      m_accum_gyro_z = new double[size];
+      m_offset_data_gyro_rate_x = new double[size];
+      m_offset_data_gyro_rate_y = new double[size];
+      m_offset_data_gyro_rate_z = new double[size];
       // Set acculumate count to 0
       m_accum_count = 0;
     }
@@ -548,22 +554,23 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
   public void calibrate() {
     synchronized (this) {
       int gyroAverageSize = Math.min(m_accum_count, m_avg_size);
-      double m_gyro_accum_x = 0.0;
-      double m_gyro_accum_y = 0.0;
-      double m_gyro_accum_z = 0.0;
+      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++) {
-        m_gyro_accum_x += m_accum_gyro_x[i];
-        m_gyro_accum_y += m_accum_gyro_y[i];
-        m_gyro_accum_z += m_accum_gyro_z[i];
+        accum_gyro_rate_x += m_offset_data_gyro_rate_x[i];
+        accum_gyro_rate_y += m_offset_data_gyro_rate_y[i];
+        accum_gyro_rate_z += m_offset_data_gyro_rate_z[i];
       }
-      m_gyro_offset_x = m_gyro_accum_x / gyroAverageSize;
-      m_gyro_offset_y = m_gyro_accum_y / gyroAverageSize;
-      m_gyro_offset_z = m_gyro_accum_z / gyroAverageSize;
-      m_integ_gyro_x = 0.0;
-      m_integ_gyro_y = 0.0;
-      m_integ_gyro_z = 0.0;
+      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;
       // System.out.println("Avg Size: " + gyroAverageSize + "X Off: " +
-      // m_gyro_offset_x + "Y Off: " + m_gyro_offset_y + "Z Off: " + m_gyro_offset_z);
+      // m_gyro_rate_offset_x + "Y Off: " + m_gyro_rate_offset_y + "Z Off: " +
+      // m_gyro_rate_offset_z);
     }
   }
 
@@ -610,9 +617,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
   /** {@inheritDoc} */
   public void reset() {
     synchronized (this) {
-      m_integ_gyro_x = 0.0;
-      m_integ_gyro_y = 0.0;
-      m_integ_gyro_z = 0.0;
+      m_integ_gyro_angle_x = 0.0;
+      m_integ_gyro_angle_y = 0.0;
+      m_integ_gyro_angle_z = 0.0;
     }
   }
 
@@ -658,9 +665,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
     int bufferAvgIndex = 0;
     double previous_timestamp = 0.0;
     double delta_angle = 0.0;
-    double gyro_x = 0.0;
-    double gyro_y = 0.0;
-    double gyro_z = 0.0;
+    double 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;
@@ -669,9 +676,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
     double mag_z = 0.0;
     double baro = 0.0;
     double temp = 0.0;
-    double gyro_x_si = 0.0;
-    double gyro_y_si = 0.0;
-    double gyro_z_si = 0.0;
+    double 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;
@@ -728,9 +735,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
             m_dt = ((double) buffer[i] - previous_timestamp) / 1000000.0;
 
             // Scale sensor data
-            gyro_x = (toShort(buffer[i + 5], buffer[i + 6]) * 0.04);
-            gyro_y = (toShort(buffer[i + 7], buffer[i + 8]) * 0.04);
-            gyro_z = (toShort(buffer[i + 9], buffer[i + 10]) * 0.04);
+            gyro_rate_x = (toShort(buffer[i + 5], buffer[i + 6]) * 0.04);
+            gyro_rate_y = (toShort(buffer[i + 7], buffer[i + 8]) * 0.04);
+            gyro_rate_z = (toShort(buffer[i + 9], buffer[i + 10]) * 0.04);
             accel_x = (toShort(buffer[i + 11], buffer[i + 12]) * 0.833);
             accel_y = (toShort(buffer[i + 13], buffer[i + 14]) * 0.833);
             accel_z = (toShort(buffer[i + 15], buffer[i + 16]) * 0.833);
@@ -742,9 +749,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
 
             // Convert scaled sensor data to SI units (for tilt calculations)
             // TODO: Should the unit outputs be selectable?
-            gyro_x_si = gyro_x * deg_to_rad;
-            gyro_y_si = gyro_y * deg_to_rad;
-            gyro_z_si = gyro_z * deg_to_rad;
+            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;
@@ -777,31 +784,31 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
                       Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
               accelAngleX = formatAccelRange(accelAngleX, -accel_z_si);
               accelAngleY = formatAccelRange(accelAngleY, -accel_z_si);
-              compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_y_si);
-              compAngleY = compFilterProcess(compAngleY, accelAngleY, -gyro_x_si);
+              compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
+              compAngleY = compFilterProcess(compAngleY, accelAngleY, -gyro_rate_x_si);
             }
 
             // Update global variables and state
             synchronized (this) {
               // Ignore first, integrated sample
               if (m_first_run) {
-                m_integ_gyro_x = 0.0;
-                m_integ_gyro_y = 0.0;
-                m_integ_gyro_z = 0.0;
+                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 to buffer
                 bufferAvgIndex = m_accum_count % m_avg_size;
-                m_accum_gyro_x[bufferAvgIndex] = gyro_x;
-                m_accum_gyro_y[bufferAvgIndex] = gyro_y;
-                m_accum_gyro_z[bufferAvgIndex] = gyro_z;
+                m_offset_data_gyro_rate_x[bufferAvgIndex] = gyro_rate_x;
+                m_offset_data_gyro_rate_y[bufferAvgIndex] = gyro_rate_y;
+                m_offset_data_gyro_rate_z[bufferAvgIndex] = gyro_rate_z;
                 // Increment counter
                 m_accum_count++;
               }
               if (!m_start_up_mode) {
-                m_gyro_x = gyro_x;
-                m_gyro_y = gyro_y;
-                m_gyro_z = gyro_z;
+                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;
@@ -815,9 +822,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
                 m_accelAngleX = accelAngleX * rad_to_deg;
                 m_accelAngleY = accelAngleY * rad_to_deg;
                 // Accumulate gyro for angle integration and publish to global variables
-                m_integ_gyro_x += (gyro_x - m_gyro_offset_x) * m_dt;
-                m_integ_gyro_y += (gyro_y - m_gyro_offset_y) * m_dt;
-                m_integ_gyro_z += (gyro_z - m_gyro_offset_z) * m_dt;
+                m_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;
               }
               // System.out.println("Good CRC");
             }
@@ -853,9 +860,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
         data_to_read = 0;
         previous_timestamp = 0.0;
         delta_angle = 0.0;
-        gyro_x = 0.0;
-        gyro_y = 0.0;
-        gyro_z = 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;
@@ -864,9 +871,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
         mag_z = 0.0;
         baro = 0.0;
         temp = 0.0;
-        gyro_x_si = 0.0;
-        gyro_y_si = 0.0;
-        gyro_z_si = 0.0;
+        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;
@@ -950,6 +957,20 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
     }
   }
 
+  /** @return Yaw axis angular rate in degrees per second (CCW positive) */
+  public synchronized double getRate() {
+    switch (m_yaw_axis) {
+      case kX:
+        return getGyroAngleX();
+      case kY:
+        return getGyroAngleY();
+      case kZ:
+        return getGyroAngleZ();
+      default:
+        return 0.0;
+    }
+  }
+
   /** @return Yaw Axis */
   public IMUAxis getYawAxis() {
     return m_yaw_axis;
@@ -960,7 +981,7 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
     if (m_simGyroAngleX != null) {
       return m_simGyroAngleX.get();
     }
-    return m_integ_gyro_x;
+    return m_integ_gyro_angle_x;
   }
 
   /** @return accumulated gyro angle in the Y axis in degrees */
@@ -968,7 +989,7 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
     if (m_simGyroAngleY != null) {
       return m_simGyroAngleY.get();
     }
-    return m_integ_gyro_y;
+    return m_integ_gyro_angle_y;
   }
 
   /** @return accumulated gyro angle in the Z axis in degrees */
@@ -976,7 +997,31 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
     if (m_simGyroAngleZ != null) {
       return m_simGyroAngleZ.get();
     }
-    return m_integ_gyro_z;
+    return m_integ_gyro_angle_z;
+  }
+
+  /** @return gyro angular rate in the X axis in degrees per second */
+  public synchronized double getGyroRateX() {
+    if (m_simGyroRateX != null) {
+      return m_simGyroRateX.get();
+    }
+    return m_gyro_rate_x;
+  }
+
+  /** @return gyro angular rate in the Y axis in degrees per second */
+  public synchronized double getGyroRateY() {
+    if (m_simGyroRateY != null) {
+      return m_simGyroRateY.get();
+    }
+    return m_gyro_rate_y;
+  }
+
+  /** @return gyro angular rate in the Z axis in degrees per second */
+  public synchronized double getGyroRateZ() {
+    if (m_simGyroRateZ != null) {
+      return m_simGyroRateZ.get();
+    }
+    return m_gyro_rate_z;
   }
 
   /** @return urrent acceleration in the X axis in meters per second squared */

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

@@ -215,9 +215,9 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable {
   private IMUAxis m_yaw_axis;
 
   // Instant raw outputs
-  private double m_gyro_x = 0.0;
-  private double m_gyro_y = 0.0;
-  private double m_gyro_z = 0.0;
+  private double m_gyro_rate_x = 0.0;
+  private double m_gyro_rate_y = 0.0;
+  private double m_gyro_rate_z = 0.0;
   private double m_accel_x = 0.0;
   private double m_accel_y = 0.0;
   private double m_accel_z = 0.0;
@@ -255,6 +255,9 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable {
   private SimDouble m_simGyroAngleX;
   private SimDouble m_simGyroAngleY;
   private SimDouble m_simGyroAngleZ;
+  private SimDouble m_simGyroRateX;
+  private SimDouble m_simGyroRateY;
+  private SimDouble m_simGyroRateZ;
   private SimDouble m_simAccelX;
   private SimDouble m_simAccelY;
   private SimDouble m_simAccelZ;
@@ -293,6 +296,9 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable {
       m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0);
       m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0);
       m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0);
       m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0);
       m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0);
       m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0);
@@ -681,15 +687,15 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable {
     int data_to_read = 0;
     double previous_timestamp = 0.0;
     double delta_angle = 0.0;
-    double gyro_x = 0.0;
-    double gyro_y = 0.0;
-    double gyro_z = 0.0;
+    double 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_x_si = 0.0;
-    double gyro_y_si = 0.0;
-    double gyro_z_si = 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;
@@ -758,18 +764,18 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable {
           delta_angle =
               (toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf)
                   / (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
-          gyro_x = (toShort(buffer[i + 7], buffer[i + 8]) / 10.0);
-          gyro_y = (toShort(buffer[i + 9], buffer[i + 10]) / 10.0);
-          gyro_z = (toShort(buffer[i + 11], buffer[i + 12]) / 10.0);
+          gyro_rate_x = (toShort(buffer[i + 7], buffer[i + 8]) / 10.0);
+          gyro_rate_y = (toShort(buffer[i + 9], buffer[i + 10]) / 10.0);
+          gyro_rate_z = (toShort(buffer[i + 11], buffer[i + 12]) / 10.0);
           accel_x = (toShort(buffer[i + 13], buffer[i + 14]) / 800.0);
           accel_y = (toShort(buffer[i + 15], buffer[i + 16]) / 800.0);
           accel_z = (toShort(buffer[i + 17], buffer[i + 18]) / 800.0);
 
           // Convert scaled sensor data to SI units (for tilt calculations)
           // TODO: Should the unit outputs be selectable?
-          gyro_x_si = gyro_x * deg_to_rad;
-          gyro_y_si = gyro_y * deg_to_rad;
-          gyro_z_si = gyro_z * deg_to_rad;
+          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;
@@ -799,8 +805,8 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable {
                     accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
             accelAngleX = formatAccelRange(accelAngleX, accel_z_si);
             accelAngleY = formatAccelRange(accelAngleY, accel_z_si);
-            compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_y_si);
-            compAngleY = compFilterProcess(compAngleY, accelAngleY, gyro_x_si);
+            compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
+            compAngleY = compFilterProcess(compAngleY, accelAngleY, gyro_rate_x_si);
           }
 
           synchronized (this) {
@@ -814,9 +820,9 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable {
             } else {
               m_integ_angle += delta_angle;
             }
-            m_gyro_x = gyro_x;
-            m_gyro_y = gyro_y;
-            m_gyro_z = gyro_z;
+            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;
@@ -834,15 +840,15 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable {
         data_to_read = 0;
         previous_timestamp = 0.0;
         delta_angle = 0.0;
-        gyro_x = 0.0;
-        gyro_y = 0.0;
-        gyro_z = 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_x_si = 0.0;
-        gyro_y_si = 0.0;
-        gyro_z_si = 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;
@@ -934,6 +940,28 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable {
     return m_integ_angle;
   }
 
+  /** @return Yaw axis angular rate in degrees per second (CCW positive) */
+  public synchronized double getRate() {
+    if (m_yaw_axis == IMUAxis.kX) {
+      if (m_simGyroRateX != null) {
+        return m_simGyroRateX.get();
+      }
+      return m_gyro_rate_x;
+    } else if (m_yaw_axis == IMUAxis.kY) {
+      if (m_simGyroRateY != null) {
+        return m_simGyroRateY.get();
+      }
+      return m_gyro_rate_y;
+    } else if (m_yaw_axis == IMUAxis.kZ) {
+      if (m_simGyroRateZ != null) {
+        return m_simGyroRateZ.get();
+      }
+      return m_gyro_rate_z;
+    } else {
+      return 0.0;
+    }
+  }
+
   /** @return Yaw Axis */
   public IMUAxis getYawAxis() {
     return m_yaw_axis;

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

@@ -13,6 +13,9 @@ public class ADIS16448_IMUSim {
   private final SimDouble m_simGyroAngleX;
   private final SimDouble m_simGyroAngleY;
   private final SimDouble m_simGyroAngleZ;
+  private final SimDouble m_simGyroRateX;
+  private final SimDouble m_simGyroRateY;
+  private final SimDouble m_simGyroRateZ;
   private final SimDouble m_simAccelX;
   private final SimDouble m_simAccelY;
   private final SimDouble m_simAccelZ;
@@ -27,6 +30,9 @@ public class ADIS16448_IMUSim {
     m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
     m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
     m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
+    m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x");
+    m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y");
+    m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z");
     m_simAccelX = wrappedSimDevice.getDouble("accel_x");
     m_simAccelY = wrappedSimDevice.getDouble("accel_y");
     m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
@@ -59,6 +65,33 @@ public class ADIS16448_IMUSim {
     m_simGyroAngleZ.set(angleDegrees);
   }
 
+  /**
+   * Sets the X axis angle in degrees per second (CCW positive).
+   *
+   * @param angularRateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateX(double angularRateDegreesPerSecond) {
+    m_simGyroRateX.set(angularRateDegreesPerSecond);
+  }
+
+  /**
+   * Sets the Y axis angle in degrees per second (CCW positive).
+   *
+   * @param angularRateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateY(double angularRateDegreesPerSecond) {
+    m_simGyroRateY.set(angularRateDegreesPerSecond);
+  }
+
+  /**
+   * Sets the Z axis angle in degrees per second (CCW positive).
+   *
+   * @param angularRateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateZ(double angularRateDegreesPerSecond) {
+    m_simGyroRateZ.set(angularRateDegreesPerSecond);
+  }
+
   /**
    * Sets the X axis acceleration in meters per second squared.
    *

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

@@ -13,6 +13,9 @@ public class ADIS16470_IMUSim {
   private final SimDouble m_simGyroAngleX;
   private final SimDouble m_simGyroAngleY;
   private final SimDouble m_simGyroAngleZ;
+  private final SimDouble m_simGyroRateX;
+  private final SimDouble m_simGyroRateY;
+  private final SimDouble m_simGyroRateZ;
   private final SimDouble m_simAccelX;
   private final SimDouble m_simAccelY;
   private final SimDouble m_simAccelZ;
@@ -27,6 +30,9 @@ public class ADIS16470_IMUSim {
     m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
     m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
     m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
+    m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x");
+    m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y");
+    m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z");
     m_simAccelX = wrappedSimDevice.getDouble("accel_x");
     m_simAccelY = wrappedSimDevice.getDouble("accel_y");
     m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
@@ -59,6 +65,33 @@ public class ADIS16470_IMUSim {
     m_simGyroAngleZ.set(angleDegrees);
   }
 
+  /**
+   * Sets the X axis angle in degrees per second (CCW positive).
+   *
+   * @param angularRateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateX(double angularRateDegreesPerSecond) {
+    m_simGyroRateX.set(angularRateDegreesPerSecond);
+  }
+
+  /**
+   * Sets the Y axis angle in degrees per second (CCW positive).
+   *
+   * @param angularRateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateY(double angularRateDegreesPerSecond) {
+    m_simGyroRateY.set(angularRateDegreesPerSecond);
+  }
+
+  /**
+   * Sets the Z axis angle in degrees per second (CCW positive).
+   *
+   * @param angularRateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateZ(double angularRateDegreesPerSecond) {
+    m_simGyroRateZ.set(angularRateDegreesPerSecond);
+  }
+
   /**
    * Sets the X axis acceleration in meters per second squared.
    *

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/digitalcommunication/Main.java

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.digitalcommunication;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

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

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.digitalcommunication;
+
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+/**
+ * This is a sample program demonstrating how to communicate to a light controller from the robot
+ * code using the roboRIO's DIO ports.
+ */
+public class Robot extends TimedRobot {
+  // define ports for digitalcommunication with light controller
+  private static final int kAlliancePort = 0;
+  private static final int kEnabledPort = 1;
+  private static final int kAutonomousPort = 2;
+  private static final int kAlertPort = 3;
+
+  private final DigitalOutput m_allianceOutput = new DigitalOutput(kAlliancePort);
+  private final DigitalOutput m_enabledOutput = new DigitalOutput(kEnabledPort);
+  private final DigitalOutput m_autonomousOutput = new DigitalOutput(kAutonomousPort);
+  private final DigitalOutput m_alertOutput = new DigitalOutput(kAlertPort);
+
+  @Override
+  public void robotPeriodic() {
+    // pull alliance port high if on red alliance, pull low if on blue alliance
+    m_allianceOutput.set(DriverStation.getAlliance() == DriverStation.Alliance.Red);
+
+    // pull enabled port high if enabled, low if disabled
+    m_enabledOutput.set(DriverStation.isEnabled());
+
+    // pull auto port high if in autonomous, low if in teleop (or disabled)
+    m_autonomousOutput.set(DriverStation.isAutonomous());
+
+    // pull alert port high if match time remaining is between 30 and 25 seconds
+    var matchTime = DriverStation.getMatchTime();
+    m_alertOutput.set(matchTime <= 30 && matchTime >= 25);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -770,5 +770,27 @@
     "gradlebase": "javaromi",
     "mainclass": "Main",
     "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": "java",
+    "commandversion": 2,
+    "mainclass": "Main"
+  },
+  {
+    "name": "I2C Communication Sample",
+    "description": "An example program that communicates with external devices (such as an Arduino) using the roboRIO's I2C port",
+    "tags": [
+      "I2C"
+    ],
+    "foldername": "i2ccommunication",
+    "gradlebase": "java",
+    "commandversion": 2,
+    "mainclass": "Main"
   }
 ]

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/i2ccommunication/Main.java

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.i2ccommunication;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

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

@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.i2ccommunication;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+/**
+ * This is a sample program demonstrating how to communicate to a light controller from the robot
+ * code using the roboRIO's I2C port.
+ */
+public class Robot extends TimedRobot {
+  private static int kDeviceAddress = 4;
+
+  private static I2C m_arduino = new I2C(I2C.Port.kOnboard, kDeviceAddress);
+
+  private void writeString(String input) {
+    // Creates a char array from the input string
+    char[] chars = input.toCharArray();
+
+    // Creates a byte array from the char array
+    byte[] data = new byte[chars.length];
+
+    // Adds each character
+    for (int i = 0; i < chars.length; i++) {
+      data[i] = (byte) chars[i];
+    }
+
+    // Writes bytes over I2C
+    m_arduino.transaction(data, data.length, null, 0);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // 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.
+    StringBuilder stateMessage = new StringBuilder(6);
+
+    stateMessage
+        .append(DriverStation.getAlliance() == DriverStation.Alliance.Red ? "R" : "B")
+        .append(DriverStation.isEnabled() ? "E" : "D")
+        .append(DriverStation.isAutonomous() ? "A" : "T")
+        .append(String.format("%03d", (int) DriverStation.getMatchTime()));
+
+    writeString(stateMessage.toString());
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/RomiDrivetrain.java

@@ -31,6 +31,12 @@ public class RomiDrivetrain {
     m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
     m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
     resetEncoders();
+
+    // Invert right side since motor is flipped
+    m_rightMotor.setInverted(true);
+
+    // Disable motor safety
+    m_diffDrive.setSafetyEnabled(false);
   }
 
   public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json

@@ -1,4 +1,15 @@
 [
+  {
+    "name": "Command Robot",
+    "description": "Command style",
+    "tags": [
+      "Command"
+    ],
+    "foldername": "commandbased",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
   {
     "name": "Timed Robot",
     "description": "Timed style",
@@ -35,13 +46,14 @@
     "commandversion": 2
   },
   {
-    "name": "Command Robot",
-    "description": "Command style",
+    "name": "Romi - Command Robot",
+    "description": "Romi - Command style",
     "tags": [
-      "Command"
+      "Command",
+      "Romi"
     ],
-    "foldername": "commandbased",
-    "gradlebase": "java",
+    "foldername": "romicommandbased",
+    "gradlebase": "javaromi",
     "mainclass": "Main",
     "commandversion": 2
   },
@@ -57,18 +69,6 @@
     "mainclass": "Main",
     "commandversion": 2
   },
-  {
-    "name": "Romi - Command Robot",
-    "description": "Romi - Command style",
-    "tags": [
-      "Command",
-      "Romi"
-    ],
-    "foldername": "romicommandbased",
-    "gradlebase": "javaromi",
-    "mainclass": "Main",
-    "commandversion": 2
-  },
   {
     "name": "Educational Robot",
     "description": "Educational Robot - Not for competition use",