SCRIPT Move java files

wpilibj/src/main/java/org/wpilib/ExpansionHub.java

@@ -0,0 +1,187 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.BooleanSubscriber;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/** This class controls a REV ExpansionHub plugged in over USB to Systemcore. */
+public class ExpansionHub implements AutoCloseable {
+  private static class DataStore implements AutoCloseable {
+    public final int m_usbId;
+    private int m_refCount;
+    private int m_reservedMotorMask;
+    private int m_reservedServoMask;
+    private final Object m_reserveLock = new Object();
+
+    private final BooleanSubscriber m_hubConnectedSubscriber;
+
+    DataStore(int usbId) {
+      m_usbId = usbId;
+      m_storeMap[usbId] = this;
+
+      NetworkTableInstance systemServer = SystemServer.getSystemServer();
+
+      m_hubConnectedSubscriber =
+          systemServer.getBooleanTopic("/rhsp/" + usbId + "/connected").subscribe(false);
+    }
+
+    @Override
+    public void close() {
+      m_storeMap[m_usbId] = null;
+    }
+
+    public void addRef() {
+      m_refCount++;
+    }
+
+    public void removeRef() {
+      m_refCount--;
+      if (m_refCount == 0) {
+        this.close();
+      }
+    }
+  }
+
+  private static final DataStore[] m_storeMap = new DataStore[4];
+
+  private static void checkUsbId(int usbId) {
+    if (usbId < 0 || usbId > 3) {
+      throw new IllegalArgumentException("USB Port " + usbId + " out of range");
+    }
+  }
+
+  private static DataStore getForUsbId(int usbId) {
+    checkUsbId(usbId);
+    synchronized (m_storeMap) {
+      DataStore store = m_storeMap[usbId];
+      if (store == null) {
+        store = new DataStore(usbId);
+      }
+      store.addRef();
+      return store;
+    }
+  }
+
+  private static void freeHub(DataStore store) {
+    synchronized (m_storeMap) {
+      store.removeRef();
+    }
+  }
+
+  private final DataStore m_dataStore;
+
+  /**
+   * Constructs a new ExpansionHub for a given USB ID
+   *
+   * <p>Multiple instances can be constructed, but will point to the same backing object with a ref
+   * count.
+   *
+   * @param usbId The USB Port ID the hub is plugged into.
+   */
+  public ExpansionHub(int usbId) {
+    m_dataStore = getForUsbId(usbId);
+  }
+
+  /**
+   * Closes an ExpansionHub object. Will not close any other instances until the last instance is
+   * closed.
+   */
+  @Override
+  public void close() {
+    freeHub(m_dataStore);
+  }
+
+  boolean checkServoChannel(int channel) {
+    return channel >= 0 && channel < 6;
+  }
+
+  boolean checkAndReserveServo(int channel) {
+    int mask = 1 << channel;
+    synchronized (m_dataStore.m_reserveLock) {
+      if ((m_dataStore.m_reservedServoMask & mask) != 0) {
+        return false;
+      }
+      m_dataStore.m_reservedServoMask |= mask;
+      return true;
+    }
+  }
+
+  void unreserveServo(int channel) {
+    int mask = 1 << channel;
+    synchronized (m_dataStore.m_reserveLock) {
+      m_dataStore.m_reservedServoMask &= ~mask;
+    }
+  }
+
+  boolean checkMotorChannel(int channel) {
+    return channel >= 0 && channel < 4;
+  }
+
+  boolean checkAndReserveMotor(int channel) {
+    int mask = 1 << channel;
+    synchronized (m_dataStore.m_reserveLock) {
+      if ((m_dataStore.m_reservedMotorMask & mask) != 0) {
+        return false;
+      }
+      m_dataStore.m_reservedMotorMask |= mask;
+      return true;
+    }
+  }
+
+  void unreserveMotor(int channel) {
+    int mask = 1 << channel;
+    synchronized (m_dataStore.m_reserveLock) {
+      m_dataStore.m_reservedMotorMask &= ~mask;
+    }
+  }
+
+  void reportUsage(String device, String data) {
+    HAL.reportUsage("ExpansionHub[" + m_dataStore.m_usbId + "]/" + device, data);
+  }
+
+  /**
+   * Constructs a servo at the requested channel on this hub.
+   *
+   * <p>Only a single instance of each servo per hub can be constructed at a time.
+   *
+   * @param channel The servo channel
+   * @return Servo object
+   */
+  public ExpansionHubServo makeServo(int channel) {
+    return new ExpansionHubServo(m_dataStore.m_usbId, channel);
+  }
+
+  /**
+   * Constructs a motor at the requested channel on this hub.
+   *
+   * <p>Only a single instance of each motor per hub can be constructed at a time.
+   *
+   * @param channel The motor channel
+   * @return Motor object
+   */
+  public ExpansionHubMotor makeMotor(int channel) {
+    return new ExpansionHubMotor(m_dataStore.m_usbId, channel);
+  }
+
+  /**
+   * Gets if the hub is currently connected over USB.
+   *
+   * @return True if hub connection, otherwise false
+   */
+  public boolean isHubConnected() {
+    return m_dataStore.m_hubConnectedSubscriber.get(false);
+  }
+
+  /**
+   * Gets the USB ID of this hub.
+   *
+   * @return The USB ID
+   */
+  public int getUsbId() {
+    return m_dataStore.m_usbId;
+  }
+}

wpilibj/src/main/java/org/wpilib/ExpansionHubMotor.java

@@ -0,0 +1,308 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.networktables.BooleanPublisher;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.IntegerPublisher;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.PubSubOption;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+
+/** This class controls a specific motor and encoder hooked up to an ExpansionHub. */
+public class ExpansionHubMotor implements AutoCloseable {
+  private static final int kPercentageMode = 0;
+  private static final int kVoltageMode = 1;
+  private static final int kPositionMode = 2;
+  private static final int kVelocityMode = 3;
+  private static final int kFollowerMode = 4;
+
+  private ExpansionHub m_hub;
+  private final int m_channel;
+
+  private final DoubleSubscriber m_encoderSubscriber;
+  private final DoubleSubscriber m_encoderVelocitySubscriber;
+  private final DoubleSubscriber m_currentSubscriber;
+
+  private final DoublePublisher m_setpointPublisher;
+  private final BooleanPublisher m_floatOn0Publisher;
+  private final BooleanPublisher m_enabledPublisher;
+
+  private final IntegerPublisher m_modePublisher;
+
+  private final BooleanPublisher m_reversedPublisher;
+  private final BooleanPublisher m_resetEncoderPublisher;
+
+  private final DoublePublisher m_distancePerCountPublisher;
+
+  private final ExpansionHubPidConstants m_velocityPidConstants;
+  private final ExpansionHubPidConstants m_positionPidConstants;
+
+  /**
+   * Constructs a servo at the requested channel on a specific USB port.
+   *
+   * @param usbId The USB port ID the hub is connected to
+   * @param channel The motor channel
+   */
+  public ExpansionHubMotor(int usbId, int channel) {
+    m_hub = new ExpansionHub(usbId);
+    m_channel = channel;
+
+    if (!m_hub.checkMotorChannel(channel)) {
+      m_hub.close();
+      throw new IllegalArgumentException("Channel " + channel + " out of range");
+    }
+
+    if (!m_hub.checkAndReserveMotor(channel)) {
+      m_hub.close();
+      throw new AllocationException("ExpansionHub Motor already allocated");
+    }
+
+    m_hub.reportUsage("ExHubMotor[" + channel + "]", "ExHubMotor");
+
+    NetworkTableInstance systemServer = SystemServer.getSystemServer();
+
+    PubSubOption[] options =
+        new PubSubOption[] {
+          PubSubOption.sendAll(true),
+          PubSubOption.keepDuplicates(true),
+          PubSubOption.periodic(0.005)
+        };
+
+    m_encoderSubscriber =
+        systemServer
+            .getDoubleTopic("/rhsp/" + usbId + "/motor" + channel + "/encoder")
+            .subscribe(0, options);
+    m_encoderVelocitySubscriber =
+        systemServer
+            .getDoubleTopic("/rhsp/" + usbId + "/motor" + channel + "/encoderVelocity")
+            .subscribe(0, options);
+    m_currentSubscriber =
+        systemServer
+            .getDoubleTopic("/rhsp/" + usbId + "/motor" + channel + "/current")
+            .subscribe(0, options);
+
+    m_setpointPublisher =
+        systemServer
+            .getDoubleTopic("/rhsp/" + usbId + "/motor" + channel + "/setpoint")
+            .publish(options);
+
+    m_distancePerCountPublisher =
+        systemServer
+            .getDoubleTopic("/rhsp/" + usbId + "/motor" + channel + "/distancePerCount")
+            .publish(options);
+
+    m_floatOn0Publisher =
+        systemServer
+            .getBooleanTopic("/rhsp/" + usbId + "/motor" + channel + "/floatOn0")
+            .publish(options);
+    m_enabledPublisher =
+        systemServer
+            .getBooleanTopic("/rhsp/" + usbId + "/motor" + channel + "/enabled")
+            .publish(options);
+
+    m_modePublisher =
+        systemServer
+            .getIntegerTopic("/rhsp/" + usbId + "/motor" + channel + "/mode")
+            .publish(options);
+
+    m_reversedPublisher =
+        systemServer
+            .getBooleanTopic("/rhsp/" + usbId + "/motor" + channel + "/reversed")
+            .publish(options);
+
+    m_resetEncoderPublisher =
+        systemServer
+            .getBooleanTopic("/rhsp/" + usbId + "/motor" + channel + "/resetEncoder")
+            .publish(options);
+
+    m_velocityPidConstants = new ExpansionHubPidConstants(usbId, channel, true);
+    m_positionPidConstants = new ExpansionHubPidConstants(usbId, channel, false);
+  }
+
+  /** Closes a motor so another instance can be constructed. */
+  @Override
+  public void close() {
+    m_hub.unreserveMotor(m_channel);
+    m_hub.close();
+    m_hub = null;
+
+    m_encoderSubscriber.close();
+    m_encoderVelocitySubscriber.close();
+    m_currentSubscriber.close();
+    m_setpointPublisher.close();
+    m_floatOn0Publisher.close();
+    m_enabledPublisher.close();
+    m_modePublisher.close();
+    m_reversedPublisher.close();
+    m_resetEncoderPublisher.close();
+    m_distancePerCountPublisher.close();
+
+    m_velocityPidConstants.close();
+    m_positionPidConstants.close();
+  }
+
+  /**
+   * Sets the percentage power to run the motor at, between -1 and 1.
+   *
+   * @param power The power to drive the motor at
+   */
+  public void setPercentagePower(double power) {
+    m_modePublisher.set(kPercentageMode);
+    m_setpointPublisher.set(power);
+  }
+
+  /**
+   * Sets the voltage to run the motor at. This value will be continously scaled to match the input
+   * voltage.
+   *
+   * @param voltage The voltage to drive the motor at
+   */
+  public void setVoltage(Voltage voltage) {
+    m_modePublisher.set(kVoltageMode);
+    m_setpointPublisher.set(voltage.in(Volts));
+  }
+
+  /**
+   * Command the motor to drive to a specific position setpoint. This value will be scaled by
+   * setDistancePerCount and influenced by the PID constants.
+   *
+   * @param setpoint The position setpoint to drive the motor to
+   */
+  public void setPositionSetpoint(double setpoint) {
+    m_modePublisher.set(kPositionMode);
+    m_setpointPublisher.set(setpoint);
+  }
+
+  /**
+   * Command the motor to drive to a specific velocity setpoint. This value will be scaled by
+   * setDistancePerCount and influenced by the PID constants.
+   *
+   * @param setpoint The velocity setpoint to drive the motor to
+   */
+  public void setVelocitySetpoint(double setpoint) {
+    m_modePublisher.set(kVelocityMode);
+    m_setpointPublisher.set(setpoint);
+  }
+
+  /**
+   * Sets if the motor output is enabled or not. Defaults to false.
+   *
+   * @param enabled True to enable, false to disable
+   */
+  public void setEnabled(boolean enabled) {
+    m_enabledPublisher.set(enabled);
+  }
+
+  /**
+   * Sets if the motor should float or brake when 0 is commanded. Defaults to false.
+   *
+   * @param floatOn0 True to float when commanded 0, false to brake
+   */
+  public void setFloatOn0(boolean floatOn0) {
+    m_floatOn0Publisher.set(floatOn0);
+  }
+
+  /**
+   * Gets the current being pulled by the motor.
+   *
+   * @return Motor current
+   */
+  public Current getCurrent() {
+    return Amps.of(m_currentSubscriber.get(0));
+  }
+
+  /**
+   * Sets the distance per count of the encoder. Used to scale encoder readings.
+   *
+   * @param perCount The distance moved per count of the encoder
+   */
+  public void setDistancePerCount(double perCount) {
+    m_distancePerCountPublisher.set(perCount);
+  }
+
+  /**
+   * Gets if the underlying ExpansionHub is connected.
+   *
+   * @return True if hub is connected, otherwise false
+   */
+  public boolean isHubConnected() {
+    return m_hub.isHubConnected();
+  }
+
+  /**
+   * Gets the current velocity of the motor encoder. Scaled into distancePerCount units.
+   *
+   * @return Encoder velocity
+   */
+  public double getEncoderVelocity() {
+    return m_encoderVelocitySubscriber.get(0);
+  }
+
+  /**
+   * Gets the current position of the motor encoder. Scaled into distancePerCount units.
+   *
+   * @return Encoder position
+   */
+  public double getEncoderPosition() {
+    return m_encoderSubscriber.get(0);
+  }
+
+  /**
+   * Sets if the motor and encoder should be reversed.
+   *
+   * @param reversed True to reverse encoder, false otherwise
+   */
+  public void setReversed(boolean reversed) {
+    m_reversedPublisher.set(reversed);
+  }
+
+  /** Reset the encoder count to 0. */
+  public void resetEncoder() {
+    m_resetEncoderPublisher.set(true);
+  }
+
+  /**
+   * Gets the PID constants object for velocity PID.
+   *
+   * @return Velocity PID constants object
+   */
+  public ExpansionHubPidConstants getVelocityPidConstants() {
+    return m_velocityPidConstants;
+  }
+
+  /**
+   * Gets the PID constants object for position PID.
+   *
+   * @return Position PID constants object
+   */
+  public ExpansionHubPidConstants getPositionPidConstants() {
+    return m_positionPidConstants;
+  }
+
+  /**
+   * Sets this motor to follow another motor on the same hub.
+   *
+   * <p>This does not support following motors that are also followers. Additionally, the direction
+   * of both motors will be the same.
+   *
+   * @param leader The motor to follow
+   */
+  public void follow(ExpansionHubMotor leader) {
+    requireNonNullParam(leader, "leader", "follow");
+    if (leader.m_hub.getUsbId() != this.m_hub.getUsbId()) {
+      throw new IllegalArgumentException("Leader motor must be on the same hub as the follower");
+    }
+    m_modePublisher.set(kFollowerMode);
+    m_setpointPublisher.set(leader.m_channel);
+  }
+}

wpilibj/src/main/java/org/wpilib/ExpansionHubPidConstants.java

@@ -0,0 +1,166 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.networktables.BooleanPublisher;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.PubSubOption;
+
+/** This class contains PID constants for an ExpansionHub motor. */
+public class ExpansionHubPidConstants {
+  private final DoublePublisher m_pPublisher;
+  private final DoublePublisher m_iPublisher;
+  private final DoublePublisher m_dPublisher;
+  private final DoublePublisher m_sPublisher;
+  private final DoublePublisher m_vPublisher;
+  private final DoublePublisher m_aPublisher;
+
+  private final BooleanPublisher m_continuousPublisher;
+  private final DoublePublisher m_continuousMinimumPublisher;
+  private final DoublePublisher m_continuousMaximumPublisher;
+
+  ExpansionHubPidConstants(int hubNumber, int motorNumber, boolean isVelocityPid) {
+    NetworkTableInstance systemServer = SystemServer.getSystemServer();
+
+    PubSubOption[] options =
+        new PubSubOption[] {
+          PubSubOption.sendAll(true),
+          PubSubOption.keepDuplicates(true),
+          PubSubOption.periodic(0.005)
+        };
+
+    String pidType = isVelocityPid ? "velocity" : "position";
+
+    m_pPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kp")
+            .publish(options);
+
+    m_iPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ki")
+            .publish(options);
+
+    m_dPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kd")
+            .publish(options);
+
+    m_aPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ka")
+            .publish(options);
+
+    m_vPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kv")
+            .publish(options);
+
+    m_sPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ks")
+            .publish(options);
+
+    m_continuousPublisher =
+        systemServer
+            .getBooleanTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/continuous")
+            .publish(options);
+
+    m_continuousMinimumPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/"
+                    + hubNumber
+                    + "/motor"
+                    + motorNumber
+                    + "/pid/"
+                    + pidType
+                    + "/continuousMinimum")
+            .publish(options);
+
+    m_continuousMaximumPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/"
+                    + hubNumber
+                    + "/motor"
+                    + motorNumber
+                    + "/pid/"
+                    + pidType
+                    + "/continousMaximum")
+            .publish(options);
+  }
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * <p>Set the proportional, integral, and differential coefficients.
+   *
+   * @param p The proportional coefficient.
+   * @param i The integral coefficient.
+   * @param d The derivative coefficient.
+   */
+  public void setPID(double p, double i, double d) {
+    m_pPublisher.set(p);
+    m_iPublisher.set(i);
+    m_dPublisher.set(d);
+  }
+
+  /**
+   * Sets the feed forward gains to the specified values.
+   *
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * <p>The PID control period is 10ms
+   *
+   * @param s The static gain in volts.
+   * @param v The velocity gain in V/(units/s).
+   * @param a The acceleration gain in V/(units/s²).
+   */
+  public void setFF(double s, double v, double a) {
+    m_sPublisher.set(s);
+    m_vPublisher.set(v);
+    m_aPublisher.set(a);
+  }
+
+  /**
+   * Enables continuous input.
+   *
+   * <p>Rather then using the max and min input range as constraints, it considers them to be the
+   * same point and automatically calculates the shortest route to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public void enableContinuousInput(double minimumInput, double maximumInput) {
+    m_continuousMaximumPublisher.set(maximumInput);
+    m_continuousMinimumPublisher.set(minimumInput);
+    m_continuousPublisher.set(true);
+  }
+
+  /** Disable continous input mode. */
+  public void disableContinuousInput() {
+    m_continuousPublisher.set(false);
+  }
+
+  void close() {
+    m_iPublisher.close();
+    m_dPublisher.close();
+    m_sPublisher.close();
+    m_vPublisher.close();
+    m_aPublisher.close();
+    m_continuousPublisher.close();
+    m_continuousMinimumPublisher.close();
+    m_continuousMaximumPublisher.close();
+  }
+}

wpilibj/src/main/java/org/wpilib/ExpansionHubServo.java

@@ -0,0 +1,240 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Microseconds;
+
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.networktables.BooleanPublisher;
+import edu.wpi.first.networktables.IntegerPublisher;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.PubSubOption;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Time;
+
+/** This class controls a specific servo hooked up to an ExpansionHub. */
+public class ExpansionHubServo implements AutoCloseable {
+  private ExpansionHub m_hub;
+  private final int m_channel;
+
+  private boolean m_reversed;
+  private boolean m_continousMode;
+
+  private final IntegerPublisher m_pulseWidthPublisher;
+  private final IntegerPublisher m_framePeriodPublisher;
+  private final BooleanPublisher m_enabledPublisher;
+
+  private double m_maxServoAngle = 180.0;
+  private double m_minServoAngle;
+
+  private int m_minPwm = 600;
+  private int m_maxPwm = 2400;
+
+  /**
+   * Constructs a servo at the requested channel on a specific USB port.
+   *
+   * @param usbId The USB port ID the hub is connected to
+   * @param channel The servo channel
+   */
+  public ExpansionHubServo(int usbId, int channel) {
+    m_hub = new ExpansionHub(usbId);
+    m_channel = channel;
+
+    if (!m_hub.checkServoChannel(channel)) {
+      m_hub.close();
+      throw new IllegalArgumentException("Channel " + channel + " out of range");
+    }
+
+    if (!m_hub.checkAndReserveServo(channel)) {
+      m_hub.close();
+      throw new AllocationException("ExpansionHub Servo already allocated");
+    }
+
+    m_hub.reportUsage("ExHubServo[" + channel + "]", "ExHubServo");
+
+    NetworkTableInstance systemServer = SystemServer.getSystemServer();
+
+    PubSubOption[] options =
+        new PubSubOption[] {
+          PubSubOption.sendAll(true),
+          PubSubOption.keepDuplicates(true),
+          PubSubOption.periodic(0.005)
+        };
+
+    m_pulseWidthPublisher =
+        systemServer
+            .getIntegerTopic("/rhsp/" + usbId + "/servo" + channel + "/pulseWidth")
+            .publish(options);
+
+    m_pulseWidthPublisher.set(1500);
+
+    m_framePeriodPublisher =
+        systemServer
+            .getIntegerTopic("/rhsp/" + usbId + "/servo" + channel + "/framePeriod")
+            .publish(options);
+
+    m_framePeriodPublisher.set(20000);
+
+    m_enabledPublisher =
+        systemServer
+            .getBooleanTopic("/rhsp/" + usbId + "/servo" + channel + "/enabled")
+            .publish(options);
+  }
+
+  /**
+   * Set the servo position.
+   *
+   * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right. If
+   * continuous rotation mode is enabled, the range is -1.0 to 1.0.
+   *
+   * @param value Position from 0.0 to 1.0 (-1 to 1 in CR mode).
+   */
+  public void set(double value) {
+    if (m_continousMode) {
+      value = Math.clamp(value, -1.0, 1.0);
+      value = (value + 1.0) / 2.0;
+    }
+
+    value = Math.clamp(value, 0.0, 1.0);
+
+    if (m_reversed) {
+      value = 1.0 - value;
+    }
+
+    int rawValue = (int) ((value * getFullRangeScaleFactor()) + m_minPwm);
+
+    m_pulseWidthPublisher.set(rawValue);
+  }
+
+  /**
+   * Sets the servo angle
+   *
+   * <p>Servo angles range defaults to 0 to 180 degrees, but can be changed with setAngleRange().
+   *
+   * @param angle Position in angle units. Will be scaled between the current angle range.
+   */
+  public void setAngle(Angle angle) {
+    double dAngle = angle.in(Degrees);
+    if (dAngle < m_minServoAngle) {
+      dAngle = m_minServoAngle;
+    } else if (dAngle > m_maxServoAngle) {
+      dAngle = m_maxServoAngle;
+    }
+
+    set((dAngle - m_minServoAngle) / getServoAngleRange());
+  }
+
+  private double getFullRangeScaleFactor() {
+    return m_maxPwm - m_minPwm;
+  }
+
+  private double getServoAngleRange() {
+    return m_maxServoAngle - m_minServoAngle;
+  }
+
+  /**
+   * Sets the raw pulse width output on the servo.
+   *
+   * @param pulseWidth Pulse width
+   */
+  public void setPulseWidth(Time pulseWidth) {
+    m_pulseWidthPublisher.set((long) pulseWidth.in(Microseconds));
+  }
+
+  /**
+   * Sets if the servo output is enabled or not. Defaults to false.
+   *
+   * @param enabled True to enable, false to disable
+   */
+  public void setEnabled(boolean enabled) {
+    m_enabledPublisher.set(enabled);
+  }
+
+  /**
+   * Sets the frame period for the servo. Defaults to 20ms.
+   *
+   * @param framePeriod The frame period
+   */
+  public void setFramePeriod(Time framePeriod) {
+    m_framePeriodPublisher.set((long) framePeriod.in(Microseconds));
+  }
+
+  /**
+   * Gets if the underlying ExpansionHub is connected.
+   *
+   * @return True if hub is connected, otherwise false
+   */
+  public boolean isHubConnected() {
+    return m_hub.isHubConnected();
+  }
+
+  /**
+   * Sets whether the servo is reversed.
+   *
+   * <p>This will reverse both set() and setAngle().
+   *
+   * @param reversed True to reverse, false for normal
+   */
+  public void setReversed(boolean reversed) {
+    m_reversed = reversed;
+  }
+
+  /**
+   * Sets the PWM range for the servo. By default, this is 600 to 2400 microseconds.
+   *
+   * <p>Maximum must be greater than minimum.
+   *
+   * @param minPwm Minimum PWM
+   * @param maxPwm Maximum PWM
+   */
+  public void setPWMRange(int minPwm, int maxPwm) {
+    if (maxPwm <= minPwm) {
+      throw new IllegalArgumentException("Maximum PWM must be greater than minimum PWM");
+    }
+    m_minPwm = minPwm;
+    m_maxPwm = maxPwm;
+  }
+
+  /**
+   * Sets the angle range for the setAngle call. By default, this is 0 to 180 degrees.
+   *
+   * <p>Maximum angle must be greater than minimum angle.
+   *
+   * @param minAngle Minimum angle
+   * @param maxAngle Maximum angle
+   */
+  public void setAngleRange(double minAngle, double maxAngle) {
+    if (maxAngle <= minAngle) {
+      throw new IllegalArgumentException("Maximum angle must be greater than minimum angle");
+    }
+    m_minServoAngle = minAngle;
+    m_maxServoAngle = maxAngle;
+  }
+
+  /**
+   * Enables or disables continuous rotation mode.
+   *
+   * <p>In continuous rotation mode, the servo will interpret Set() commands to between -1.0 and
+   * 1.0, instead of 0.0 to 1.0.
+   *
+   * @param enable True to enable continuous rotation mode, false to disable
+   */
+  public void setContinousRotationMode(boolean enable) {
+    m_continousMode = enable;
+  }
+
+  /** Closes a servo so another instance can be constructed. */
+  @Override
+  public void close() {
+    m_hub.unreserveServo(m_channel);
+    m_hub.close();
+    m_hub = null;
+
+    m_pulseWidthPublisher.close();
+    m_framePeriodPublisher.close();
+    m_enabledPublisher.close();
+  }
+}

wpilibj/src/main/java/org/wpilib/counter/EdgeConfiguration.java

@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.counter;
+
+/** Edge configuration. */
+public enum EdgeConfiguration {
+  /** Rising edge configuration. */
+  kRisingEdge(true),
+  /** Falling edge configuration. */
+  kFallingEdge(false);
+
+  /** True if triggering on rising edge. */
+  @SuppressWarnings("MemberName")
+  public final boolean rising;
+
+  EdgeConfiguration(boolean rising) {
+    this.rising = rising;
+  }
+}

wpilibj/src/main/java/org/wpilib/counter/Tachometer.java

@@ -0,0 +1,139 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.counter;
+
+import edu.wpi.first.hal.CounterJNI;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Tachometer.
+ *
+ * <p>The Tachometer class measures the time between digital pulses to determine the rotation speed
+ * of a mechanism. Examples of devices that could be used with the tachometer class are a hall
+ * effect sensor, break beam sensor, or optical sensor detecting tape on a shooter wheel. Unlike
+ * encoders, this class only needs a single digital input.
+ */
+public class Tachometer implements Sendable, AutoCloseable {
+  private final int m_handle;
+  private int m_edgesPerRevolution = 1;
+
+  /**
+   * Constructs a new tachometer.
+   *
+   * @param channel The channel of the Tachometer.
+   * @param configuration The edge configuration
+   */
+  @SuppressWarnings("this-escape")
+  public Tachometer(int channel, EdgeConfiguration configuration) {
+    m_handle = CounterJNI.initializeCounter(channel, configuration.rising);
+
+    HAL.reportUsage("IO", channel, "Tachometer");
+    SendableRegistry.add(this, "Tachometer", channel);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    CounterJNI.freeCounter(m_handle);
+  }
+
+  /**
+   * Gets the tachometer period.
+   *
+   * @return Current period (in seconds).
+   */
+  public double getPeriod() {
+    return CounterJNI.getCounterPeriod(m_handle);
+  }
+
+  /**
+   * Gets the tachometer frequency.
+   *
+   * @return Current frequency (in hertz).
+   */
+  public double getFrequency() {
+    double period = getPeriod();
+    if (period == 0) {
+      return 0;
+    }
+    return 1 / period;
+  }
+
+  /**
+   * Gets the number of edges per revolution.
+   *
+   * @return Edges per revolution.
+   */
+  public int getEdgesPerRevolution() {
+    return m_edgesPerRevolution;
+  }
+
+  /**
+   * Sets the number of edges per revolution.
+   *
+   * @param edgesPerRevolution Edges per revolution.
+   */
+  public void setEdgesPerRevolution(int edgesPerRevolution) {
+    m_edgesPerRevolution = edgesPerRevolution;
+  }
+
+  /**
+   * Gets the current tachometer revolutions per second.
+   *
+   * <p>setEdgesPerRevolution must be set with a non 0 value for this to return valid values.
+   *
+   * @return Current RPS.
+   */
+  public double getRevolutionsPerSecond() {
+    double period = getPeriod();
+    if (period == 0) {
+      return 0;
+    }
+    int edgesPerRevolution = getEdgesPerRevolution();
+    if (edgesPerRevolution == 0) {
+      return 0;
+    }
+    return (1.0 / edgesPerRevolution) / period;
+  }
+
+  /**
+   * Gets the current tachometer revolutions per minute.
+   *
+   * <p>setEdgesPerRevolution must be set with a non 0 value for this to return valid values.
+   *
+   * @return Current RPM.
+   */
+  public double getRevolutionsPerMinute() {
+    return getRevolutionsPerSecond() * 60;
+  }
+
+  /**
+   * Gets if the tachometer is stopped.
+   *
+   * @return True if the tachometer is stopped.
+   */
+  public boolean getStopped() {
+    return CounterJNI.getCounterStopped(m_handle);
+  }
+
+  /**
+   * Sets the maximum period before the tachometer is considered stopped.
+   *
+   * @param maxPeriod The max period (in seconds).
+   */
+  public void setMaxPeriod(double maxPeriod) {
+    CounterJNI.setCounterMaxPeriod(m_handle, maxPeriod);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Tachometer");
+    builder.addDoubleProperty("RPS", this::getRevolutionsPerSecond, null);
+    builder.addDoubleProperty("RPM", this::getRevolutionsPerMinute, null);
+  }
+}

wpilibj/src/main/java/org/wpilib/counter/UpDownCounter.java

@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.counter;
+
+import edu.wpi.first.hal.CounterJNI;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Up Down Counter.
+ *
+ * <p>This class can count edges on a single digital input or count up based on an edge from one
+ * digital input and down on an edge from another digital input.
+ */
+public class UpDownCounter implements Sendable, AutoCloseable {
+  private final int m_handle;
+
+  /**
+   * Constructs a new UpDown Counter.
+   *
+   * @param channel The up count source (can be null).
+   * @param configuration The edge configuration.
+   */
+  @SuppressWarnings("this-escape")
+  public UpDownCounter(int channel, EdgeConfiguration configuration) {
+    m_handle = CounterJNI.initializeCounter(channel, configuration.rising);
+
+    reset();
+
+    HAL.reportUsage("IO", channel, "UpDownCounter");
+    SendableRegistry.add(this, "UpDown Counter", channel);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    CounterJNI.freeCounter(m_handle);
+  }
+
+  /**
+   * Sets the configuration for the up source.
+   *
+   * @param configuration The up source configuration.
+   */
+  public void setEdgeConfiguration(EdgeConfiguration configuration) {
+    CounterJNI.setCounterEdgeConfiguration(m_handle, configuration.rising);
+  }
+
+  /** Resets the current count. */
+  public final void reset() {
+    CounterJNI.resetCounter(m_handle);
+  }
+
+  /**
+   * Gets the current count.
+   *
+   * @return The current count.
+   */
+  public int getCount() {
+    return CounterJNI.getCounter(m_handle);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("UpDown Counter");
+    builder.addDoubleProperty("Count", this::getCount, null);
+  }
+}

wpilibj/src/main/java/org/wpilib/drive/DifferentialDrive.java

@@ -0,0 +1,368 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.drive;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import java.util.function.DoubleConsumer;
+
+/**
+ * A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive
+ * base, "tank drive", or West Coast Drive.
+ *
+ * <p>These drive bases typically have drop-center / skid-steer with two or more wheels per side
+ * (e.g., 6WD or 8WD). This class takes a setter per side. For four and six motor drivetrains, use
+ * CAN motor controller followers or {@link
+ * edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}.
+ *
+ * <p>A differential drive robot has left and right wheels separated by an arbitrary width.
+ *
+ * <p>Drive base diagram:
+ *
+ * <pre>
+ * |_______|
+ * | |   | |
+ *   |   |
+ * |_|___|_|
+ * |       |
+ * </pre>
+ *
+ * <p>Each drive function provides different inverse kinematic relations for a differential drive
+ * robot.
+ *
+ * <p>This library uses the NWU axes convention (North-West-Up as external reference in the world
+ * frame). The positive X axis points ahead, the positive Y axis points to the left, and the
+ * positive Z axis points up. Rotations follow the right-hand rule, so counterclockwise rotation
+ * around the Z axis is positive.
+ *
+ * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
+ * be set to 0, and larger values will be scaled so that the full range is still used. This deadband
+ * value can be changed with {@link #setDeadband}.
+ *
+ * <p>{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. The tankDrive, arcadeDrive,
+ * or curvatureDrive methods should be called periodically to avoid Motor Safety timeouts.
+ */
+public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
+  private static int instances;
+
+  private final DoubleConsumer m_leftMotor;
+  private final DoubleConsumer m_rightMotor;
+
+  // Used for Sendable property getters
+  private double m_leftOutput;
+  private double m_rightOutput;
+
+  private boolean m_reported;
+
+  /**
+   * Wheel speeds for a differential drive.
+   *
+   * <p>Uses normalized voltage [-1.0..1.0].
+   */
+  @SuppressWarnings("MemberName")
+  public static class WheelSpeeds {
+    /** Left wheel speed. */
+    public double left;
+
+    /** Right wheel speed. */
+    public double right;
+
+    /** Constructs a WheelSpeeds with zeroes for left and right speeds. */
+    public WheelSpeeds() {}
+
+    /**
+     * Constructs a WheelSpeeds.
+     *
+     * @param left The left speed [-1.0..1.0].
+     * @param right The right speed [-1.0..1.0].
+     */
+    public WheelSpeeds(double left, double right) {
+      this.left = left;
+      this.right = right;
+    }
+  }
+
+  /**
+   * Construct a DifferentialDrive.
+   *
+   * <p>To pass multiple motors per side, use CAN motor controller followers or {@link
+   * edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}. If a
+   * motor needs to be inverted, do so before passing it in.
+   *
+   * @param leftMotor Left motor.
+   * @param rightMotor Right motor.
+   */
+  @SuppressWarnings({"removal", "this-escape"})
+  public DifferentialDrive(MotorController leftMotor, MotorController rightMotor) {
+    this((double output) -> leftMotor.set(output), (double output) -> rightMotor.set(output));
+    SendableRegistry.addChild(this, leftMotor);
+    SendableRegistry.addChild(this, rightMotor);
+  }
+
+  /**
+   * Construct a DifferentialDrive.
+   *
+   * <p>To pass multiple motors per side, use CAN motor controller followers or {@link
+   * edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}. If a
+   * motor needs to be inverted, do so before passing it in.
+   *
+   * @param leftMotor Left motor setter.
+   * @param rightMotor Right motor setter.
+   */
+  @SuppressWarnings("this-escape")
+  public DifferentialDrive(DoubleConsumer leftMotor, DoubleConsumer rightMotor) {
+    requireNonNullParam(leftMotor, "leftMotor", "DifferentialDrive");
+    requireNonNullParam(rightMotor, "rightMotor", "DifferentialDrive");
+
+    m_leftMotor = leftMotor;
+    m_rightMotor = rightMotor;
+    instances++;
+    SendableRegistry.add(this, "DifferentialDrive", instances);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Arcade drive method for differential drive platform. The calculated values will be squared to
+   * decrease sensitivity at low speeds.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
+   *     positive.
+   */
+  public void arcadeDrive(double xSpeed, double zRotation) {
+    arcadeDrive(xSpeed, zRotation, true);
+  }
+
+  /**
+   * Arcade drive method for differential drive platform.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
+   *     positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   */
+  public void arcadeDrive(double xSpeed, double zRotation, boolean squareInputs) {
+    if (!m_reported) {
+      HAL.reportUsage("RobotDrive", "DifferentialArcade");
+      m_reported = true;
+    }
+
+    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
+    zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
+
+    var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs);
+
+    m_leftOutput = speeds.left * m_maxOutput;
+    m_rightOutput = speeds.right * m_maxOutput;
+
+    m_leftMotor.accept(m_leftOutput);
+    m_rightMotor.accept(m_rightOutput);
+
+    feed();
+  }
+
+  /**
+   * Curvature drive method for differential drive platform.
+   *
+   * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
+   * heading change. This makes the robot more controllable at high speeds.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive.
+   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
+   *     maneuvers. zRotation will control turning rate instead of curvature.
+   */
+  public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInPlace) {
+    if (!m_reported) {
+      HAL.reportUsage("RobotDrive", "DifferentialCurvature");
+      m_reported = true;
+    }
+
+    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
+    zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
+
+    var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
+
+    m_leftOutput = speeds.left * m_maxOutput;
+    m_rightOutput = speeds.right * m_maxOutput;
+
+    m_leftMotor.accept(m_leftOutput);
+    m_rightMotor.accept(m_rightOutput);
+
+    feed();
+  }
+
+  /**
+   * Tank drive method for differential drive platform. The calculated values will be squared to
+   * decrease sensitivity at low speeds.
+   *
+   * @param leftSpeed The robot's left side speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param rightSpeed The robot's right side speed along the X axis [-1.0..1.0]. Forward is
+   *     positive.
+   */
+  public void tankDrive(double leftSpeed, double rightSpeed) {
+    tankDrive(leftSpeed, rightSpeed, true);
+  }
+
+  /**
+   * Tank drive method for differential drive platform.
+   *
+   * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
+   *     positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   */
+  public void tankDrive(double leftSpeed, double rightSpeed, boolean squareInputs) {
+    if (!m_reported) {
+      HAL.reportUsage("RobotDrive", "DifferentialTank");
+      m_reported = true;
+    }
+
+    leftSpeed = MathUtil.applyDeadband(leftSpeed, m_deadband);
+    rightSpeed = MathUtil.applyDeadband(rightSpeed, m_deadband);
+
+    var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs);
+
+    m_leftOutput = speeds.left * m_maxOutput;
+    m_rightOutput = speeds.right * m_maxOutput;
+
+    m_leftMotor.accept(m_leftOutput);
+    m_rightMotor.accept(m_rightOutput);
+
+    feed();
+  }
+
+  /**
+   * Arcade drive inverse kinematics for differential drive platform.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
+   *     positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   * @return Wheel speeds [-1.0..1.0].
+   */
+  public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) {
+    xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
+    zRotation = Math.clamp(zRotation, -1.0, 1.0);
+
+    // Square the inputs (while preserving the sign) to increase fine control
+    // while permitting full power.
+    if (squareInputs) {
+      xSpeed = MathUtil.copyDirectionPow(xSpeed, 2);
+      zRotation = MathUtil.copyDirectionPow(zRotation, 2);
+    }
+
+    double leftSpeed = xSpeed - zRotation;
+    double rightSpeed = xSpeed + zRotation;
+
+    // Find the maximum possible value of (throttle + turn) along the vector
+    // that the joystick is pointing, then desaturate the wheel speeds
+    double greaterInput = Math.max(Math.abs(xSpeed), Math.abs(zRotation));
+    double lesserInput = Math.min(Math.abs(xSpeed), Math.abs(zRotation));
+    if (greaterInput == 0.0) {
+      return new WheelSpeeds(0.0, 0.0);
+    }
+    double saturatedInput = (greaterInput + lesserInput) / greaterInput;
+    leftSpeed /= saturatedInput;
+    rightSpeed /= saturatedInput;
+
+    return new WheelSpeeds(leftSpeed, rightSpeed);
+  }
+
+  /**
+   * Curvature drive inverse kinematics for differential drive platform.
+   *
+   * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
+   * heading change. This makes the robot more controllable at high speeds.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive.
+   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
+   *     maneuvers. zRotation will control rotation rate around the Z axis instead of curvature.
+   * @return Wheel speeds [-1.0..1.0].
+   */
+  public static WheelSpeeds curvatureDriveIK(
+      double xSpeed, double zRotation, boolean allowTurnInPlace) {
+    xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
+    zRotation = Math.clamp(zRotation, -1.0, 1.0);
+
+    double leftSpeed;
+    double rightSpeed;
+
+    if (allowTurnInPlace) {
+      leftSpeed = xSpeed - zRotation;
+      rightSpeed = xSpeed + zRotation;
+    } else {
+      leftSpeed = xSpeed - Math.abs(xSpeed) * zRotation;
+      rightSpeed = xSpeed + Math.abs(xSpeed) * zRotation;
+    }
+
+    // Desaturate wheel speeds
+    double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
+    if (maxMagnitude > 1.0) {
+      leftSpeed /= maxMagnitude;
+      rightSpeed /= maxMagnitude;
+    }
+
+    return new WheelSpeeds(leftSpeed, rightSpeed);
+  }
+
+  /**
+   * Tank drive inverse kinematics for differential drive platform.
+   *
+   * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
+   *     positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   * @return Wheel speeds [-1.0..1.0].
+   */
+  public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) {
+    leftSpeed = Math.clamp(leftSpeed, -1.0, 1.0);
+    rightSpeed = Math.clamp(rightSpeed, -1.0, 1.0);
+
+    // Square the inputs (while preserving the sign) to increase fine control
+    // while permitting full power.
+    if (squareInputs) {
+      leftSpeed = MathUtil.copyDirectionPow(leftSpeed, 2);
+      rightSpeed = MathUtil.copyDirectionPow(rightSpeed, 2);
+    }
+
+    return new WheelSpeeds(leftSpeed, rightSpeed);
+  }
+
+  @Override
+  public void stopMotor() {
+    m_leftOutput = 0.0;
+    m_rightOutput = 0.0;
+
+    m_leftMotor.accept(0.0);
+    m_rightMotor.accept(0.0);
+
+    feed();
+  }
+
+  @Override
+  public String getDescription() {
+    return "DifferentialDrive";
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("DifferentialDrive");
+    builder.setActuator(true);
+    builder.addDoubleProperty("Left Motor Speed", () -> m_leftOutput, m_leftMotor);
+    builder.addDoubleProperty("Right Motor Speed", () -> m_rightOutput, m_rightMotor);
+  }
+}

wpilibj/src/main/java/org/wpilib/drive/MecanumDrive.java

@@ -0,0 +1,323 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.drive;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import java.util.function.DoubleConsumer;
+
+/**
+ * A class for driving Mecanum drive platforms.
+ *
+ * <p>Mecanum drives are rectangular with one wheel on each corner. Each wheel has rollers toed in
+ * 45 degrees toward the front or back. When looking at the wheels from the top, the roller axles
+ * should form an X across the robot. Each drive() function provides different inverse kinematic
+ * relations for a Mecanum drive robot.
+ *
+ * <p>Drive base diagram:
+ *
+ * <pre>
+ * \\_______/
+ * \\ |   | /
+ *   |   |
+ * /_|___|_\\
+ * /       \\
+ * </pre>
+ *
+ * <p>Each drive() function provides different inverse kinematic relations for a Mecanum drive
+ * robot.
+ *
+ * <p>This library uses the NWU axes convention (North-West-Up as external reference in the world
+ * frame). The positive X axis points ahead, the positive Y axis points to the left, and the
+ * positive Z axis points up. Rotations follow the right-hand rule, so counterclockwise rotation
+ * around the Z axis is positive.
+ *
+ * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
+ * be set to 0, and larger values will be scaled so that the full range is still used. This deadband
+ * value can be changed with {@link #setDeadband}.
+ *
+ * <p>{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. The driveCartesian or
+ * drivePolar methods should be called periodically to avoid Motor Safety timeouts.
+ */
+public class MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable {
+  private static int instances;
+
+  private final DoubleConsumer m_frontLeftMotor;
+  private final DoubleConsumer m_rearLeftMotor;
+  private final DoubleConsumer m_frontRightMotor;
+  private final DoubleConsumer m_rearRightMotor;
+
+  // Used for Sendable property getters
+  private double m_frontLeftOutput;
+  private double m_rearLeftOutput;
+  private double m_frontRightOutput;
+  private double m_rearRightOutput;
+
+  private boolean m_reported;
+
+  /**
+   * Wheel speeds for a mecanum drive.
+   *
+   * <p>Uses normalized voltage [-1.0..1.0].
+   */
+  @SuppressWarnings("MemberName")
+  public static class WheelSpeeds {
+    /** Front-left wheel speed. */
+    public double frontLeft;
+
+    /** Front-right wheel speed. */
+    public double frontRight;
+
+    /** Rear-left wheel speed. */
+    public double rearLeft;
+
+    /** Rear-right wheel speed. */
+    public double rearRight;
+
+    /** Constructs a WheelSpeeds with zeroes for all four speeds. */
+    public WheelSpeeds() {}
+
+    /**
+     * Constructs a WheelSpeeds.
+     *
+     * @param frontLeft The front left speed [-1.0..1.0].
+     * @param frontRight The front right speed [-1.0..1.0].
+     * @param rearLeft The rear left speed [-1.0..1.0].
+     * @param rearRight The rear right speed [-1.0..1.0].
+     */
+    public WheelSpeeds(double frontLeft, double frontRight, double rearLeft, double rearRight) {
+      this.frontLeft = frontLeft;
+      this.frontRight = frontRight;
+      this.rearLeft = rearLeft;
+      this.rearRight = rearRight;
+    }
+  }
+
+  /**
+   * Construct a MecanumDrive.
+   *
+   * <p>If a motor needs to be inverted, do so before passing it in.
+   *
+   * @param frontLeftMotor The motor on the front-left corner.
+   * @param rearLeftMotor The motor on the rear-left corner.
+   * @param frontRightMotor The motor on the front-right corner.
+   * @param rearRightMotor The motor on the rear-right corner.
+   */
+  @SuppressWarnings({"removal", "this-escape"})
+  public MecanumDrive(
+      MotorController frontLeftMotor,
+      MotorController rearLeftMotor,
+      MotorController frontRightMotor,
+      MotorController rearRightMotor) {
+    this(
+        (double output) -> frontLeftMotor.set(output),
+        (double output) -> rearLeftMotor.set(output),
+        (double output) -> frontRightMotor.set(output),
+        (double output) -> rearRightMotor.set(output));
+    SendableRegistry.addChild(this, frontLeftMotor);
+    SendableRegistry.addChild(this, rearLeftMotor);
+    SendableRegistry.addChild(this, frontRightMotor);
+    SendableRegistry.addChild(this, rearRightMotor);
+  }
+
+  /**
+   * Construct a MecanumDrive.
+   *
+   * <p>If a motor needs to be inverted, do so before passing it in.
+   *
+   * @param frontLeftMotor The setter for the motor on the front-left corner.
+   * @param rearLeftMotor The setter for the motor on the rear-left corner.
+   * @param frontRightMotor The setter for the motor on the front-right corner.
+   * @param rearRightMotor The setter for the motor on the rear-right corner.
+   */
+  @SuppressWarnings("this-escape")
+  public MecanumDrive(
+      DoubleConsumer frontLeftMotor,
+      DoubleConsumer rearLeftMotor,
+      DoubleConsumer frontRightMotor,
+      DoubleConsumer rearRightMotor) {
+    requireNonNullParam(frontLeftMotor, "frontLeftMotor", "MecanumDrive");
+    requireNonNullParam(rearLeftMotor, "rearLeftMotor", "MecanumDrive");
+    requireNonNullParam(frontRightMotor, "frontRightMotor", "MecanumDrive");
+    requireNonNullParam(rearRightMotor, "rearRightMotor", "MecanumDrive");
+
+    m_frontLeftMotor = frontLeftMotor;
+    m_rearLeftMotor = rearLeftMotor;
+    m_frontRightMotor = frontRightMotor;
+    m_rearRightMotor = rearRightMotor;
+    instances++;
+    SendableRegistry.add(this, "MecanumDrive", instances);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Drive method for Mecanum platform.
+   *
+   * <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
+   * independent of its angle or rotation rate.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
+   *     positive.
+   */
+  public void driveCartesian(double xSpeed, double ySpeed, double zRotation) {
+    driveCartesian(xSpeed, ySpeed, zRotation, Rotation2d.kZero);
+  }
+
+  /**
+   * Drive method for Mecanum platform.
+   *
+   * <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
+   * independent of its angle or rotation rate.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
+   *     positive.
+   * @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented
+   *     controls.
+   */
+  public void driveCartesian(double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
+    if (!m_reported) {
+      HAL.reportUsage("RobotDrive", "MecanumCartesian");
+      m_reported = true;
+    }
+
+    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
+    ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
+
+    var speeds = driveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
+
+    m_frontLeftOutput = speeds.frontLeft * m_maxOutput;
+    m_rearLeftOutput = speeds.rearLeft * m_maxOutput;
+    m_frontRightOutput = speeds.frontRight * m_maxOutput;
+    m_rearRightOutput = speeds.rearRight * m_maxOutput;
+
+    m_frontLeftMotor.accept(m_frontLeftOutput);
+    m_frontRightMotor.accept(m_frontRightOutput);
+    m_rearLeftMotor.accept(m_rearLeftOutput);
+    m_rearRightMotor.accept(m_rearRightOutput);
+
+    feed();
+  }
+
+  /**
+   * Drive method for Mecanum platform.
+   *
+   * <p>Angles are measured counterclockwise from straight ahead. The speed at which the robot
+   * drives (translation) is independent of its angle or rotation rate.
+   *
+   * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
+   * @param angle The gyro heading around the Z axis at which the robot drives.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
+   *     positive.
+   */
+  public void drivePolar(double magnitude, Rotation2d angle, double zRotation) {
+    if (!m_reported) {
+      HAL.reportUsage("RobotDrive", "MecanumPolar");
+      m_reported = true;
+    }
+
+    driveCartesian(
+        magnitude * angle.getCos(), magnitude * angle.getSin(), zRotation, Rotation2d.kZero);
+  }
+
+  /**
+   * Cartesian inverse kinematics for Mecanum platform.
+   *
+   * <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
+   * independent of its angle or rotation rate.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
+   *     positive.
+   * @return Wheel speeds [-1.0..1.0].
+   */
+  public static WheelSpeeds driveCartesianIK(double xSpeed, double ySpeed, double zRotation) {
+    return driveCartesianIK(xSpeed, ySpeed, zRotation, Rotation2d.kZero);
+  }
+
+  /**
+   * Cartesian inverse kinematics for Mecanum platform.
+   *
+   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent of
+   * its angle or rotation rate.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
+   *     positive.
+   * @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented
+   *     controls.
+   * @return Wheel speeds [-1.0..1.0].
+   */
+  public static WheelSpeeds driveCartesianIK(
+      double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
+    xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
+    ySpeed = Math.clamp(ySpeed, -1.0, 1.0);
+
+    // Compensate for gyro angle.
+    var input = new Translation2d(xSpeed, ySpeed).rotateBy(gyroAngle.unaryMinus());
+
+    double[] wheelSpeeds = new double[4];
+    wheelSpeeds[MotorType.kFrontLeft.value] = input.getX() + input.getY() + zRotation;
+    wheelSpeeds[MotorType.kFrontRight.value] = input.getX() - input.getY() - zRotation;
+    wheelSpeeds[MotorType.kRearLeft.value] = input.getX() - input.getY() + zRotation;
+    wheelSpeeds[MotorType.kRearRight.value] = input.getX() + input.getY() - zRotation;
+
+    normalize(wheelSpeeds);
+
+    return new WheelSpeeds(
+        wheelSpeeds[MotorType.kFrontLeft.value],
+        wheelSpeeds[MotorType.kFrontRight.value],
+        wheelSpeeds[MotorType.kRearLeft.value],
+        wheelSpeeds[MotorType.kRearRight.value]);
+  }
+
+  @Override
+  public void stopMotor() {
+    m_frontLeftOutput = 0.0;
+    m_frontRightOutput = 0.0;
+    m_rearLeftOutput = 0.0;
+    m_rearRightOutput = 0.0;
+
+    m_frontLeftMotor.accept(0.0);
+    m_frontRightMotor.accept(0.0);
+    m_rearLeftMotor.accept(0.0);
+    m_rearRightMotor.accept(0.0);
+
+    feed();
+  }
+
+  @Override
+  public String getDescription() {
+    return "MecanumDrive";
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("MecanumDrive");
+    builder.setActuator(true);
+    builder.addDoubleProperty("Front Left Motor Speed", () -> m_frontLeftOutput, m_frontLeftMotor);
+    builder.addDoubleProperty(
+        "Front Right Motor Speed", () -> m_frontRightOutput, m_frontRightMotor);
+    builder.addDoubleProperty("Rear Left Motor Speed", () -> m_rearLeftOutput, m_rearLeftMotor);
+    builder.addDoubleProperty("Rear Right Motor Speed", () -> m_rearRightOutput, m_rearRightMotor);
+  }
+}

wpilibj/src/main/java/org/wpilib/drive/RobotDriveBase.java

@@ -0,0 +1,117 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.drive;
+
+import edu.wpi.first.wpilibj.MotorSafety;
+
+/**
+ * Common base class for drive platforms.
+ *
+ * <p>{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default.
+ */
+public abstract class RobotDriveBase extends MotorSafety {
+  /** Default input deadband. */
+  public static final double kDefaultDeadband = 0.02;
+
+  /** Default maximum output. */
+  public static final double kDefaultMaxOutput = 1.0;
+
+  /** Input deadband. */
+  protected double m_deadband = kDefaultDeadband;
+
+  /** Maximum output. */
+  protected double m_maxOutput = kDefaultMaxOutput;
+
+  /** The location of a motor on the robot for the purpose of driving. */
+  public enum MotorType {
+    /** Front left motor. */
+    kFrontLeft(0),
+    /** Front right motor. */
+    kFrontRight(1),
+    /** Rear left motor. */
+    kRearLeft(2),
+    /** Rear right motor. */
+    kRearRight(3),
+    /** Left motor. */
+    kLeft(0),
+    /** Right motor. */
+    kRight(1),
+    /** Back motor. */
+    kBack(2);
+
+    /** MotorType value. */
+    public final int value;
+
+    MotorType(int value) {
+      this.value = value;
+    }
+  }
+
+  /** RobotDriveBase constructor. */
+  @SuppressWarnings("this-escape")
+  public RobotDriveBase() {
+    setSafetyEnabled(true);
+  }
+
+  /**
+   * Sets the deadband applied to the drive inputs (e.g., joystick values).
+   *
+   * <p>The default value is {@value #kDefaultDeadband}. Inputs smaller than the deadband are set to
+   * 0.0 while inputs larger than the deadband are scaled from 0.0 to 1.0. See {@link
+   * edu.wpi.first.math.MathUtil#applyDeadband}.
+   *
+   * @param deadband The deadband to set.
+   */
+  public void setDeadband(double deadband) {
+    m_deadband = deadband;
+  }
+
+  /**
+   * Configure the scaling factor for using drive methods with motor controllers in a mode other
+   * than PercentVbus or to limit the maximum output.
+   *
+   * <p>The default value is {@value #kDefaultMaxOutput}.
+   *
+   * @param maxOutput Multiplied with the output percentage computed by the drive functions.
+   */
+  public void setMaxOutput(double maxOutput) {
+    m_maxOutput = maxOutput;
+  }
+
+  /**
+   * Feed the motor safety object. Resets the timer that will stop the motors if it completes.
+   *
+   * @see MotorSafety#feed()
+   */
+  public void feedWatchdog() {
+    feed();
+  }
+
+  @Override
+  public abstract void stopMotor();
+
+  @Override
+  public abstract String getDescription();
+
+  /**
+   * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+   *
+   * @param wheelSpeeds List of wheel speeds to normalize.
+   */
+  protected static void normalize(double[] wheelSpeeds) {
+    double maxMagnitude = Math.abs(wheelSpeeds[0]);
+    for (int i = 1; i < wheelSpeeds.length; i++) {
+      double temp = Math.abs(wheelSpeeds[i]);
+      if (maxMagnitude < temp) {
+        maxMagnitude = temp;
+      }
+    }
+    if (maxMagnitude > 1.0) {
+      for (int i = 0; i < wheelSpeeds.length; i++) {
+        wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+      }
+    }
+  }
+}

wpilibj/src/main/java/org/wpilib/driverstation/DSControlWord.java

@@ -0,0 +1,126 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.ControlWord;
+
+/** A wrapper around Driver Station control word. */
+public class DSControlWord {
+  private final ControlWord m_controlWord = new ControlWord();
+
+  /**
+   * DSControlWord constructor.
+   *
+   * <p>Upon construction, the current Driver Station control word is read and stored internally.
+   */
+  public DSControlWord() {
+    refresh();
+  }
+
+  /** Update internal Driver Station control word. */
+  public final void refresh() {
+    DriverStation.refreshControlWordFromCache(m_controlWord);
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be enabled.
+   *
+   * @return True if the robot is enabled, false otherwise.
+   */
+  public boolean isEnabled() {
+    return m_controlWord.getEnabled() && m_controlWord.getDSAttached();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be disabled.
+   *
+   * @return True if the robot should be disabled, false otherwise.
+   */
+  public boolean isDisabled() {
+    return !isEnabled();
+  }
+
+  /**
+   * Gets a value indicating whether the Robot is e-stopped.
+   *
+   * @return True if the robot is e-stopped, false otherwise.
+   */
+  public boolean isEStopped() {
+    return m_controlWord.getEStop();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * autonomous mode.
+   *
+   * @return True if autonomous mode should be enabled, false otherwise.
+   */
+  public boolean isAutonomous() {
+    return m_controlWord.getAutonomous();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * autonomous mode and enabled.
+   *
+   * @return True if autonomous should be set and the robot should be enabled.
+   */
+  public boolean isAutonomousEnabled() {
+    return m_controlWord.getAutonomous()
+        && m_controlWord.getEnabled()
+        && m_controlWord.getDSAttached();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * operator-controlled mode.
+   *
+   * @return True if operator-controlled mode should be enabled, false otherwise.
+   */
+  public boolean isTeleop() {
+    return !(isAutonomous() || isTest());
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * operator-controller mode and enabled.
+   *
+   * @return True if operator-controlled mode should be set and the robot should be enabled.
+   */
+  public boolean isTeleopEnabled() {
+    return !m_controlWord.getAutonomous()
+        && !m_controlWord.getTest()
+        && m_controlWord.getEnabled()
+        && m_controlWord.getDSAttached();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in test
+   * mode.
+   *
+   * @return True if test mode should be enabled, false otherwise.
+   */
+  public boolean isTest() {
+    return m_controlWord.getTest();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station is attached.
+   *
+   * @return True if Driver Station is attached, false otherwise.
+   */
+  public boolean isDSAttached() {
+    return m_controlWord.getDSAttached();
+  }
+
+  /**
+   * Gets if the driver station attached to a Field Management System.
+   *
+   * @return true if the robot is competing on a field being controlled by a Field Management System
+   */
+  public boolean isFMSAttached() {
+    return m_controlWord.getFMSAttached();
+  }
+}

wpilibj/src/main/java/org/wpilib/driverstation/GenericHID.java

@@ -0,0 +1,500 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.wpilibj.DriverStation.POVDirection;
+import edu.wpi.first.wpilibj.event.BooleanEvent;
+import edu.wpi.first.wpilibj.event.EventLoop;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Handle input from standard HID devices connected to the Driver Station.
+ *
+ * <p>This class handles standard input that comes from the Driver Station. Each time a value is
+ * requested the most recent value is returned. There is a single class instance for each device and
+ * the mapping of ports to hardware buttons depends on the code in the Driver Station.
+ */
+public class GenericHID {
+  /** Represents a rumble output on the Joystick. */
+  public enum RumbleType {
+    /** Left rumble motor. */
+    kLeftRumble,
+    /** Right rumble motor. */
+    kRightRumble,
+    /** Both left and right rumble motors. */
+    kBothRumble
+  }
+
+  /** USB HID interface type. */
+  public enum HIDType {
+    /** Unknown. */
+    kUnknown(-1),
+    /** XInputUnknown. */
+    kXInputUnknown(0),
+    /** XInputGamepad. */
+    kXInputGamepad(1),
+    /** XInputWheel. */
+    kXInputWheel(2),
+    /** XInputArcadeStick. */
+    kXInputArcadeStick(3),
+    /** XInputFlightStick. */
+    kXInputFlightStick(4),
+    /** XInputDancePad. */
+    kXInputDancePad(5),
+    /** XInputGuitar. */
+    kXInputGuitar(6),
+    /** XInputGuitar2. */
+    kXInputGuitar2(7),
+    /** XInputDrumKit. */
+    kXInputDrumKit(8),
+    /** XInputGuitar3. */
+    kXInputGuitar3(11),
+    /** XInputArcadePad. */
+    kXInputArcadePad(19),
+    /** HIDJoystick. */
+    kHIDJoystick(20),
+    /** HIDGamepad. */
+    kHIDGamepad(21),
+    /** HIDDriving. */
+    kHIDDriving(22),
+    /** HIDFlight. */
+    kHIDFlight(23),
+    /** HID1stPerson. */
+    kHID1stPerson(24);
+
+    /** HIDType value. */
+    public final int value;
+
+    private static final Map<Integer, HIDType> map = new HashMap<>();
+
+    HIDType(int value) {
+      this.value = value;
+    }
+
+    static {
+      for (HIDType hidType : HIDType.values()) {
+        map.put(hidType.value, hidType);
+      }
+    }
+
+    /**
+     * Creates an HIDType with the given value.
+     *
+     * @param value HIDType's value.
+     * @return HIDType with the given value.
+     */
+    public static HIDType of(int value) {
+      return map.get(value);
+    }
+  }
+
+  private final int m_port;
+  private int m_outputs;
+  private int m_leftRumble;
+  private int m_rightRumble;
+  private final Map<EventLoop, Map<Integer, BooleanEvent>> m_buttonCache = new HashMap<>();
+  private final Map<EventLoop, Map<Pair<Integer, Double>, BooleanEvent>> m_axisLessThanCache =
+      new HashMap<>();
+  private final Map<EventLoop, Map<Pair<Integer, Double>, BooleanEvent>> m_axisGreaterThanCache =
+      new HashMap<>();
+  private final Map<EventLoop, Map<Integer, BooleanEvent>> m_povCache = new HashMap<>();
+
+  /**
+   * Construct an instance of a device.
+   *
+   * @param port The port index on the Driver Station that the device is plugged into.
+   */
+  public GenericHID(int port) {
+    m_port = port;
+  }
+
+  /**
+   * Get the button value (starting at button 1).
+   *
+   * <p>The buttons are returned in a single 16 bit value with one bit representing the state of
+   * each button. The appropriate button is returned as a boolean value.
+   *
+   * <p>This method returns true if the button is being held down at the time that this method is
+   * being called.
+   *
+   * @param button The button number to be read (starting at 1)
+   * @return The state of the button.
+   */
+  public boolean getRawButton(int button) {
+    return DriverStation.getStickButton(m_port, (byte) button);
+  }
+
+  /**
+   * Whether the button was pressed since the last check. Button indexes begin at 1.
+   *
+   * <p>This method returns true if the button went from not pressed to held down since the last
+   * time this method was called. This is useful if you only want to call a function once when you
+   * press the button.
+   *
+   * @param button The button index, beginning at 0.
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getRawButtonPressed(int button) {
+    return DriverStation.getStickButtonPressed(m_port, (byte) button);
+  }
+
+  /**
+   * Whether the button was released since the last check. Button indexes begin at 1.
+   *
+   * <p>This method returns true if the button went from held down to not pressed since the last
+   * time this method was called. This is useful if you only want to call a function once when you
+   * release the button.
+   *
+   * @param button The button index, beginning at 0.
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getRawButtonReleased(int button) {
+    return DriverStation.getStickButtonReleased(m_port, button);
+  }
+
+  /**
+   * Constructs an event instance around this button's digital signal.
+   *
+   * @param button the button index
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the button's digital signal attached to the given loop.
+   */
+  public BooleanEvent button(int button, EventLoop loop) {
+    synchronized (m_buttonCache) {
+      var cache = m_buttonCache.computeIfAbsent(loop, k -> new HashMap<>());
+      return cache.computeIfAbsent(button, k -> new BooleanEvent(loop, () -> getRawButton(k)));
+    }
+  }
+
+  /**
+   * Get the value of the axis.
+   *
+   * @param axis The axis to read, starting at 0.
+   * @return The value of the axis.
+   */
+  public double getRawAxis(int axis) {
+    return DriverStation.getStickAxis(m_port, axis);
+  }
+
+  /**
+   * Get the angle of a POV on the HID.
+   *
+   * @param pov The index of the POV to read (starting at 0). Defaults to 0.
+   * @return the angle of the POV.
+   */
+  public POVDirection getPOV(int pov) {
+    return DriverStation.getStickPOV(m_port, pov);
+  }
+
+  /**
+   * Get the angle of the default POV (index 0) on the HID.
+   *
+   * @return the angle of the POV.
+   */
+  public POVDirection getPOV() {
+    return getPOV(0);
+  }
+
+  /**
+   * Constructs a BooleanEvent instance based around this angle of a POV on the HID.
+   *
+   * @param angle POV angle.
+   * @param loop the event loop instance to attach the event to.
+   * @return a BooleanEvent instance based around this angle of a POV on the HID.
+   */
+  public BooleanEvent pov(POVDirection angle, EventLoop loop) {
+    return pov(0, angle, loop);
+  }
+
+  /**
+   * Constructs a BooleanEvent instance based around this angle of a POV on the HID.
+   *
+   * @param pov index of the POV to read (starting at 0). Defaults to 0.
+   * @param angle POV angle.
+   * @param loop the event loop instance to attach the event to.
+   * @return a BooleanEvent instance based around this angle of a POV on the HID.
+   */
+  public BooleanEvent pov(int pov, POVDirection angle, EventLoop loop) {
+    synchronized (m_povCache) {
+      var cache = m_povCache.computeIfAbsent(loop, k -> new HashMap<>());
+      // angle.value is a 4 bit bitfield
+      return cache.computeIfAbsent(
+          pov * 16 + angle.value, k -> new BooleanEvent(loop, () -> getPOV(pov) == angle));
+    }
+  }
+
+  /**
+   * Constructs a BooleanEvent instance based around the up direction of the default (index 0) POV
+   * on the HID.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a BooleanEvent instance based around the up direction a POV on the HID.
+   */
+  public BooleanEvent povUp(EventLoop loop) {
+    return pov(POVDirection.Up, loop);
+  }
+
+  /**
+   * Constructs a BooleanEvent instance based around the up right direction of the default (index 0)
+   * POV on the HID.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a BooleanEvent instance based around the up right direction of a POV on the HID.
+   */
+  public BooleanEvent povUpRight(EventLoop loop) {
+    return pov(POVDirection.UpRight, loop);
+  }
+
+  /**
+   * Constructs a BooleanEvent instance based around the right direction of the default (index 0)
+   * POV on the HID.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a BooleanEvent instance based around the right direction of a POV on the HID.
+   */
+  public BooleanEvent povRight(EventLoop loop) {
+    return pov(POVDirection.Right, loop);
+  }
+
+  /**
+   * Constructs a BooleanEvent instance based around the down right direction of the default (index
+   * 0) POV on the HID.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a BooleanEvent instance based around the down right direction of a POV on the HID.
+   */
+  public BooleanEvent povDownRight(EventLoop loop) {
+    return pov(POVDirection.DownRight, loop);
+  }
+
+  /**
+   * Constructs a BooleanEvent instance based around the down direction of the default (index 0) POV
+   * on the HID.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a BooleanEvent instance based around the down direction of a POV on the HID.
+   */
+  public BooleanEvent povDown(EventLoop loop) {
+    return pov(POVDirection.Down, loop);
+  }
+
+  /**
+   * Constructs a BooleanEvent instance based around the down left direction of the default (index
+   * 0) POV on the HID.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a BooleanEvent instance based around the down left direction of a POV on the HID.
+   */
+  public BooleanEvent povDownLeft(EventLoop loop) {
+    return pov(POVDirection.DownLeft, loop);
+  }
+
+  /**
+   * Constructs a BooleanEvent instance based around the left direction of the default (index 0) POV
+   * on the HID.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a BooleanEvent instance based around the left direction of a POV on the HID.
+   */
+  public BooleanEvent povLeft(EventLoop loop) {
+    return pov(POVDirection.Left, loop);
+  }
+
+  /**
+   * Constructs a BooleanEvent instance based around the up left direction of the default (index 0)
+   * POV on the HID.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a BooleanEvent instance based around the up left direction of a POV on the HID.
+   */
+  public BooleanEvent povUpLeft(EventLoop loop) {
+    return pov(POVDirection.UpLeft, loop);
+  }
+
+  /**
+   * Constructs a BooleanEvent instance based around the center (not pressed) of the default (index
+   * 0) POV on the HID.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a BooleanEvent instance based around the center of a POV on the HID.
+   */
+  public BooleanEvent povCenter(EventLoop loop) {
+    return pov(POVDirection.Center, loop);
+  }
+
+  /**
+   * Constructs an event instance that is true when the axis value is less than {@code threshold},
+   * attached to the given loop.
+   *
+   * @param axis The axis to read, starting at 0
+   * @param threshold The value below which this event should return true.
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance that is true when the axis value is less than the provided threshold.
+   */
+  public BooleanEvent axisLessThan(int axis, double threshold, EventLoop loop) {
+    synchronized (m_axisLessThanCache) {
+      var cache = m_axisLessThanCache.computeIfAbsent(loop, k -> new HashMap<>());
+      return cache.computeIfAbsent(
+          Pair.of(axis, threshold),
+          k -> new BooleanEvent(loop, () -> getRawAxis(axis) < threshold));
+    }
+  }
+
+  /**
+   * Constructs an event instance that is true when the axis value is greater than {@code
+   * threshold}, attached to the given loop.
+   *
+   * @param axis The axis to read, starting at 0
+   * @param threshold The value above which this event should return true.
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance that is true when the axis value is greater than the provided
+   *     threshold.
+   */
+  public BooleanEvent axisGreaterThan(int axis, double threshold, EventLoop loop) {
+    synchronized (m_axisGreaterThanCache) {
+      var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>());
+      return cache.computeIfAbsent(
+          Pair.of(axis, threshold),
+          k -> new BooleanEvent(loop, () -> getRawAxis(axis) > threshold));
+    }
+  }
+
+  /**
+   * Get the maximum axis index for the joystick.
+   *
+   * @return the maximum axis index for the joystick
+   */
+  public int getAxesMaximumIndex() {
+    return DriverStation.getStickAxesMaximumIndex(m_port);
+  }
+
+  /**
+   * Get the number of axes for the HID.
+   *
+   * @return the number of axis for the current HID
+   */
+  public int getAxesAvailable() {
+    return DriverStation.getStickAxesAvailable(m_port);
+  }
+
+  /**
+   * Get the maximum POV index for the joystick.
+   *
+   * @return the maximum POV index for the joystick
+   */
+  public int getPOVsMaximumIndex() {
+    return DriverStation.getStickPOVsMaximumIndex(m_port);
+  }
+
+  /**
+   * For the current HID, return the number of POVs.
+   *
+   * @return the number of POVs for the current HID
+   */
+  public int getPOVsAvailable() {
+    return DriverStation.getStickPOVsAvailable(m_port);
+  }
+
+  /**
+   * Get the maximum button index for the joystick.
+   *
+   * @return the maximum button index for the joystick
+   */
+  public int getButtonsMaximumIndex() {
+    return DriverStation.getStickButtonsMaximumIndex(m_port);
+  }
+
+  /**
+   * For the current HID, return the number of buttons.
+   *
+   * @return the number of buttons for the current HID
+   */
+  public long getButtonsAvailable() {
+    return DriverStation.getStickButtonsAvailable(m_port);
+  }
+
+  /**
+   * Get if the HID is connected.
+   *
+   * @return true if the HID is connected
+   */
+  public boolean isConnected() {
+    return DriverStation.isJoystickConnected(m_port);
+  }
+
+  /**
+   * Get the type of the HID.
+   *
+   * @return the type of the HID.
+   */
+  public HIDType getType() {
+    return HIDType.of(DriverStation.getJoystickType(m_port));
+  }
+
+  /**
+   * Get the name of the HID.
+   *
+   * @return the name of the HID.
+   */
+  public String getName() {
+    return DriverStation.getJoystickName(m_port);
+  }
+
+  /**
+   * Get the port number of the HID.
+   *
+   * @return The port number of the HID.
+   */
+  public int getPort() {
+    return m_port;
+  }
+
+  /**
+   * Set a single HID output value for the HID.
+   *
+   * @param outputNumber The index of the output to set (1-32)
+   * @param value The value to set the output to
+   */
+  public void setOutput(int outputNumber, boolean value) {
+    m_outputs = (m_outputs & ~(1 << (outputNumber - 1))) | ((value ? 1 : 0) << (outputNumber - 1));
+    DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
+  }
+
+  /**
+   * Set all HID output values for the HID.
+   *
+   * @param value The 32 bit output value (1 bit for each output)
+   */
+  public void setOutputs(int value) {
+    m_outputs = value;
+    DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
+  }
+
+  /**
+   * Set the rumble output for the HID. The DS currently supports 2 rumble values, left rumble and
+   * right rumble.
+   *
+   * @param type Which rumble value to set
+   * @param value The normalized value (0 to 1) to set the rumble to
+   */
+  public void setRumble(RumbleType type, double value) {
+    value = Math.clamp(value, 0, 1);
+    int rumbleValue = (int) (value * 65535);
+    switch (type) {
+      case kLeftRumble -> this.m_leftRumble = rumbleValue;
+      case kRightRumble -> this.m_rightRumble = rumbleValue;
+      default -> {
+        this.m_leftRumble = rumbleValue;
+        this.m_rightRumble = rumbleValue;
+      }
+    }
+
+    DriverStationJNI.setJoystickOutputs(
+        (byte) this.m_port, this.m_outputs, this.m_leftRumble, this.m_rightRumble);
+  }
+}

wpilibj/src/main/java/org/wpilib/driverstation/Joystick.java

@@ -0,0 +1,341 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.event.BooleanEvent;
+import edu.wpi.first.wpilibj.event.EventLoop;
+
+/**
+ * Handle input from Flight Joysticks connected to the Driver Station.
+ *
+ * <p>This class handles standard input that comes from the Driver Station. Each time a value is
+ * requested the most recent value is returned. There is a single class instance for each joystick
+ * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
+ */
+public class Joystick extends GenericHID {
+  /** Default X axis channel. */
+  public static final byte kDefaultXChannel = 0;
+
+  /** Default Y axis channel. */
+  public static final byte kDefaultYChannel = 1;
+
+  /** Default Z axis channel. */
+  public static final byte kDefaultZChannel = 2;
+
+  /** Default twist axis channel. */
+  public static final byte kDefaultTwistChannel = 2;
+
+  /** Default throttle axis channel. */
+  public static final byte kDefaultThrottleChannel = 3;
+
+  /** Represents an analog axis on a joystick. */
+  public enum AxisType {
+    /** X axis. */
+    kX(0),
+    /** Y axis. */
+    kY(1),
+    /** Z axis. */
+    kZ(2),
+    /** Twist axis. */
+    kTwist(3),
+    /** Throttle axis. */
+    kThrottle(4);
+
+    /** AxisType value. */
+    public final int value;
+
+    AxisType(int value) {
+      this.value = value;
+    }
+  }
+
+  /** Represents a digital button on a joystick. */
+  public enum ButtonType {
+    /** kTrigger. */
+    kTrigger(1),
+    /** kTop. */
+    kTop(2);
+
+    /** ButtonType value. */
+    public final int value;
+
+    ButtonType(int value) {
+      this.value = value;
+    }
+  }
+
+  private final byte[] m_axes = new byte[AxisType.values().length];
+
+  /**
+   * Construct an instance of a joystick.
+   *
+   * @param port The port index on the Driver Station that the joystick is plugged into.
+   */
+  public Joystick(final int port) {
+    super(port);
+
+    m_axes[AxisType.kX.value] = kDefaultXChannel;
+    m_axes[AxisType.kY.value] = kDefaultYChannel;
+    m_axes[AxisType.kZ.value] = kDefaultZChannel;
+    m_axes[AxisType.kTwist.value] = kDefaultTwistChannel;
+    m_axes[AxisType.kThrottle.value] = kDefaultThrottleChannel;
+
+    HAL.reportUsage("HID", port, "Joystick");
+  }
+
+  /**
+   * Set the channel associated with the X axis.
+   *
+   * @param channel The channel to set the axis to.
+   */
+  public void setXChannel(int channel) {
+    m_axes[AxisType.kX.value] = (byte) channel;
+  }
+
+  /**
+   * Set the channel associated with the Y axis.
+   *
+   * @param channel The channel to set the axis to.
+   */
+  public void setYChannel(int channel) {
+    m_axes[AxisType.kY.value] = (byte) channel;
+  }
+
+  /**
+   * Set the channel associated with the Z axis.
+   *
+   * @param channel The channel to set the axis to.
+   */
+  public void setZChannel(int channel) {
+    m_axes[AxisType.kZ.value] = (byte) channel;
+  }
+
+  /**
+   * Set the channel associated with the throttle axis.
+   *
+   * @param channel The channel to set the axis to.
+   */
+  public void setThrottleChannel(int channel) {
+    m_axes[AxisType.kThrottle.value] = (byte) channel;
+  }
+
+  /**
+   * Set the channel associated with the twist axis.
+   *
+   * @param channel The channel to set the axis to.
+   */
+  public void setTwistChannel(int channel) {
+    m_axes[AxisType.kTwist.value] = (byte) channel;
+  }
+
+  /**
+   * Get the channel currently associated with the X axis.
+   *
+   * @return The channel for the axis.
+   */
+  public int getXChannel() {
+    return m_axes[AxisType.kX.value];
+  }
+
+  /**
+   * Get the channel currently associated with the Y axis.
+   *
+   * @return The channel for the axis.
+   */
+  public int getYChannel() {
+    return m_axes[AxisType.kY.value];
+  }
+
+  /**
+   * Get the channel currently associated with the Z axis.
+   *
+   * @return The channel for the axis.
+   */
+  public int getZChannel() {
+    return m_axes[AxisType.kZ.value];
+  }
+
+  /**
+   * Get the channel currently associated with the twist axis.
+   *
+   * @return The channel for the axis.
+   */
+  public int getTwistChannel() {
+    return m_axes[AxisType.kTwist.value];
+  }
+
+  /**
+   * Get the channel currently associated with the throttle axis.
+   *
+   * @return The channel for the axis.
+   */
+  public int getThrottleChannel() {
+    return m_axes[AxisType.kThrottle.value];
+  }
+
+  /**
+   * Get the X value of the joystick. This depends on the mapping of the joystick connected to the
+   * current port. On most joysticks, positive is to the right.
+   *
+   * @return The X value of the joystick.
+   */
+  public final double getX() {
+    return getRawAxis(m_axes[AxisType.kX.value]);
+  }
+
+  /**
+   * Get the Y value of the joystick. This depends on the mapping of the joystick connected to the
+   * current port. On most joysticks, positive is to the back.
+   *
+   * @return The Y value of the joystick.
+   */
+  public final double getY() {
+    return getRawAxis(m_axes[AxisType.kY.value]);
+  }
+
+  /**
+   * Get the z position of the HID.
+   *
+   * @return the z position
+   */
+  public final double getZ() {
+    return getRawAxis(m_axes[AxisType.kZ.value]);
+  }
+
+  /**
+   * Get the twist value of the current joystick. This depends on the mapping of the joystick
+   * connected to the current port.
+   *
+   * @return The Twist value of the joystick.
+   */
+  public final double getTwist() {
+    return getRawAxis(m_axes[AxisType.kTwist.value]);
+  }
+
+  /**
+   * Get the throttle value of the current joystick. This depends on the mapping of the joystick
+   * connected to the current port.
+   *
+   * @return The Throttle value of the joystick.
+   */
+  public final double getThrottle() {
+    return getRawAxis(m_axes[AxisType.kThrottle.value]);
+  }
+
+  /**
+   * Read the state of the trigger on the joystick.
+   *
+   * @return The state of the trigger.
+   */
+  public boolean getTrigger() {
+    return getRawButton(ButtonType.kTrigger.value);
+  }
+
+  /**
+   * Whether the trigger was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getTriggerPressed() {
+    return getRawButtonPressed(ButtonType.kTrigger.value);
+  }
+
+  /**
+   * Whether the trigger was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getTriggerReleased() {
+    return getRawButtonReleased(ButtonType.kTrigger.value);
+  }
+
+  /**
+   * Constructs an event instance around the trigger button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the trigger button's digital signal attached to the
+   *     given loop.
+   */
+  public BooleanEvent trigger(EventLoop loop) {
+    return button(ButtonType.kTrigger.value, loop);
+  }
+
+  /**
+   * Read the state of the top button on the joystick.
+   *
+   * @return The state of the top button.
+   */
+  public boolean getTop() {
+    return getRawButton(ButtonType.kTop.value);
+  }
+
+  /**
+   * Whether the top button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getTopPressed() {
+    return getRawButtonPressed(ButtonType.kTop.value);
+  }
+
+  /**
+   * Whether the top button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getTopReleased() {
+    return getRawButtonReleased(ButtonType.kTop.value);
+  }
+
+  /**
+   * Constructs an event instance around the top button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the top button's digital signal attached to the given
+   *     loop.
+   */
+  public BooleanEvent top(EventLoop loop) {
+    return button(ButtonType.kTop.value, loop);
+  }
+
+  /**
+   * Get the magnitude of the vector formed by the joystick's current position relative to its
+   * origin.
+   *
+   * @return The magnitude of the direction vector
+   */
+  public double getMagnitude() {
+    return Math.hypot(getX(), getY());
+  }
+
+  /**
+   * Get the direction of the vector formed by the joystick and its origin in radians. 0 is forward
+   * and clockwise is positive. (Straight right is π/2.)
+   *
+   * @return The direction of the vector in radians
+   */
+  public double getDirectionRadians() {
+    // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html#joystick-and-controller-coordinate-system
+    // A positive rotation around the X axis moves the joystick right, and a
+    // positive rotation around the Y axis moves the joystick backward. When
+    // treating them as translations, 0 radians is measured from the right
+    // direction, and angle increases clockwise.
+    //
+    // It's rotated 90 degrees CCW (y is negated and the arguments are reversed)
+    // so that 0 radians is forward.
+    return Math.atan2(getX(), -getY());
+  }
+
+  /**
+   * Get the direction of the vector formed by the joystick and its origin in degrees. 0 is forward
+   * and clockwise is positive. (Straight right is 90.)
+   *
+   * @return The direction of the vector in degrees
+   */
+  public double getDirectionDegrees() {
+    return Math.toDegrees(getDirectionRadians());
+  }
+}

wpilibj/src/main/java/org/wpilib/event/BooleanEvent.java

@@ -0,0 +1,222 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.event;
+
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.units.measure.Time;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.function.BiFunction;
+import java.util.function.BooleanSupplier;
+
+/**
+ * This class provides an easy way to link actions to active high logic signals. Each object
+ * represents a digital signal to which callback actions can be bound using {@link
+ * #ifHigh(Runnable)}.
+ *
+ * <p>BooleanEvents can easily be composed for advanced functionality using {@link
+ * #and(BooleanSupplier)}, {@link #or(BooleanSupplier)}, and {@link #negate()}.
+ *
+ * <p>To get a new BooleanEvent that triggers when this one changes see {@link #falling()} and
+ * {@link #rising()}.
+ */
+public class BooleanEvent implements BooleanSupplier {
+  /** Poller loop. */
+  protected final EventLoop m_loop;
+
+  private final BooleanSupplier m_signal;
+
+  /** The state of the condition in the current loop poll. */
+  private final AtomicBoolean m_state = new AtomicBoolean(false);
+
+  /**
+   * Creates a new event that is active when the condition is true.
+   *
+   * @param loop the loop that polls this event.
+   * @param signal the digital signal represented by this object.
+   */
+  public BooleanEvent(EventLoop loop, BooleanSupplier signal) {
+    m_loop = requireNonNullParam(loop, "loop", "BooleanEvent");
+    m_signal = requireNonNullParam(signal, "signal", "BooleanEvent");
+    m_state.set(m_signal.getAsBoolean());
+    m_loop.bind(() -> m_state.set(m_signal.getAsBoolean()));
+  }
+
+  /**
+   * Returns the state of this signal (high or low) as of the last loop poll.
+   *
+   * @return true for the high state, false for the low state. If the event was never polled, it
+   *     returns the state at event construction.
+   */
+  @Override
+  public final boolean getAsBoolean() {
+    return m_state.get();
+  }
+
+  /**
+   * Bind an action to this event.
+   *
+   * @param action the action to run if this event is active.
+   */
+  public final void ifHigh(Runnable action) {
+    m_loop.bind(
+        () -> {
+          if (m_state.get()) {
+            action.run();
+          }
+        });
+  }
+
+  /**
+   * A method to "downcast" a BooleanEvent instance to a subclass (for example, to a command-based
+   * version of this class).
+   *
+   * @param ctor a method reference to the constructor of the subclass that accepts the loop as the
+   *     first parameter and the condition/signal as the second.
+   * @param <T> the subclass type
+   * @return an instance of the subclass.
+   */
+  public <T extends BooleanSupplier> T castTo(BiFunction<EventLoop, BooleanSupplier, T> ctor) {
+    return ctor.apply(m_loop, m_state::get);
+  }
+
+  /**
+   * Creates a new event that is active when this event is inactive.
+   *
+   * @return the new event.
+   */
+  public BooleanEvent negate() {
+    return new BooleanEvent(m_loop, () -> !m_state.get());
+  }
+
+  /**
+   * Composes this event with another event, returning a new event that is active when both events
+   * are active.
+   *
+   * <p>The events must use the same event loop. If the events use different event loops, the
+   * composed signal won't update until both loops are polled.
+   *
+   * @param other the event to compose with.
+   * @return the new event.
+   */
+  public BooleanEvent and(BooleanSupplier other) {
+    requireNonNullParam(other, "other", "and");
+    return new BooleanEvent(m_loop, () -> m_state.get() && other.getAsBoolean());
+  }
+
+  /**
+   * Composes this event with another event, returning a new event that is active when either event
+   * is active.
+   *
+   * <p>The events must use the same event loop. If the events use different event loops, the
+   * composed signal won't update until both loops are polled.
+   *
+   * @param other the event to compose with.
+   * @return the new event.
+   */
+  public BooleanEvent or(BooleanSupplier other) {
+    requireNonNullParam(other, "other", "or");
+    return new BooleanEvent(m_loop, () -> m_state.get() || other.getAsBoolean());
+  }
+
+  /**
+   * Creates a new event that triggers when this one changes from false to true.
+   *
+   * @return the new event.
+   */
+  public BooleanEvent rising() {
+    return new BooleanEvent(
+        m_loop,
+        new BooleanSupplier() {
+          private boolean m_previous = m_state.get();
+
+          @Override
+          public boolean getAsBoolean() {
+            boolean present = m_state.get();
+            boolean ret = !m_previous && present;
+            m_previous = present;
+            return ret;
+          }
+        });
+  }
+
+  /**
+   * Creates a new event that triggers when this one changes from true to false.
+   *
+   * @return the event.
+   */
+  public BooleanEvent falling() {
+    return new BooleanEvent(
+        m_loop,
+        new BooleanSupplier() {
+          private boolean m_previous = m_state.get();
+
+          @Override
+          public boolean getAsBoolean() {
+            boolean present = m_state.get();
+            boolean ret = m_previous && !present;
+            m_previous = present;
+            return ret;
+          }
+        });
+  }
+
+  /**
+   * Creates a new debounced event from this event - it will become active when this event has been
+   * active for longer than the specified period.
+   *
+   * @param period The debounce period.
+   * @return The debounced event (rising edges debounced only).
+   */
+  public BooleanEvent debounce(Time period) {
+    return debounce(period.in(Seconds));
+  }
+
+  /**
+   * Creates a new debounced event from this event - it will become active when this event has been
+   * active for longer than the specified period.
+   *
+   * @param seconds The debounce period in seconds.
+   * @return The debounced event (rising edges debounced only).
+   */
+  public BooleanEvent debounce(double seconds) {
+    return debounce(seconds, Debouncer.DebounceType.kRising);
+  }
+
+  /**
+   * Creates a new debounced event from this event - it will become active when this event has been
+   * active for longer than the specified period.
+   *
+   * @param period The debounce period.
+   * @param type The debounce type.
+   * @return The debounced event.
+   */
+  public BooleanEvent debounce(Time period, Debouncer.DebounceType type) {
+    return debounce(period.in(Seconds), type);
+  }
+
+  /**
+   * Creates a new debounced event from this event - it will become active when this event has been
+   * active for longer than the specified period.
+   *
+   * @param seconds The debounce period in seconds.
+   * @param type The debounce type.
+   * @return The debounced event.
+   */
+  public BooleanEvent debounce(double seconds, Debouncer.DebounceType type) {
+    return new BooleanEvent(
+        m_loop,
+        new BooleanSupplier() {
+          private final Debouncer m_debouncer = new Debouncer(seconds, type);
+
+          @Override
+          public boolean getAsBoolean() {
+            return m_debouncer.calculate(m_state.get());
+          }
+        });
+  }
+}

wpilibj/src/main/java/org/wpilib/event/EventLoop.java

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.event;
+
+import java.util.Collection;
+import java.util.ConcurrentModificationException;
+import java.util.LinkedHashSet;
+
+/**
+ * A declarative way to bind a set of actions to a loop and execute them when the loop is polled.
+ */
+public final class EventLoop {
+  private final Collection<Runnable> m_bindings = new LinkedHashSet<>();
+  private boolean m_running;
+
+  /** Default constructor. */
+  public EventLoop() {}
+
+  /**
+   * Bind a new action to run when the loop is polled.
+   *
+   * @param action the action to run.
+   */
+  public void bind(Runnable action) {
+    if (m_running) {
+      throw new ConcurrentModificationException("Cannot bind EventLoop while it is running");
+    }
+    m_bindings.add(action);
+  }
+
+  /** Poll all bindings. */
+  @SuppressWarnings("PMD.UnusedAssignment")
+  public void poll() {
+    try {
+      m_running = true;
+      m_bindings.forEach(Runnable::run);
+    } finally {
+      m_running = false;
+    }
+  }
+
+  /** Clear all bindings. */
+  public void clear() {
+    if (m_running) {
+      throw new ConcurrentModificationException("Cannot clear EventLoop while it is running");
+    }
+    m_bindings.clear();
+  }
+}

wpilibj/src/main/java/org/wpilib/event/NetworkBooleanEvent.java

@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.event;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.networktables.BooleanSubscriber;
+import edu.wpi.first.networktables.BooleanTopic;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/** This class provides an easy way to link NetworkTables boolean topics to callback actions. */
+public class NetworkBooleanEvent extends BooleanEvent {
+  /**
+   * Creates a new event with the given boolean topic determining whether it is active.
+   *
+   * @param loop the loop that polls this event
+   * @param topic The boolean topic that contains the value
+   */
+  public NetworkBooleanEvent(EventLoop loop, BooleanTopic topic) {
+    this(loop, topic.subscribe(false));
+  }
+
+  /**
+   * Creates a new event with the given boolean subscriber determining whether it is active.
+   *
+   * @param loop the loop that polls this event
+   * @param sub The boolean subscriber that provides the value
+   */
+  public NetworkBooleanEvent(EventLoop loop, BooleanSubscriber sub) {
+    super(loop, () -> sub.getTopic().getInstance().isConnected() && sub.get());
+    requireNonNullParam(sub, "sub", "NetworkBooleanEvent");
+  }
+
+  /**
+   * Creates a new event with the given boolean topic determining whether it is active.
+   *
+   * @param loop the loop that polls this event
+   * @param table The NetworkTable that contains the topic
+   * @param topicName The topic name within the table that contains the value
+   */
+  public NetworkBooleanEvent(EventLoop loop, NetworkTable table, String topicName) {
+    this(loop, table.getBooleanTopic(topicName));
+  }
+
+  /**
+   * Creates a new event with the given boolean topic determining whether it is active.
+   *
+   * @param loop the loop that polls this event
+   * @param tableName The NetworkTable name that contains the topic
+   * @param topicName The topic name within the table that contains the value
+   */
+  public NetworkBooleanEvent(EventLoop loop, String tableName, String topicName) {
+    this(loop, NetworkTableInstance.getDefault(), tableName, topicName);
+  }
+
+  /**
+   * Creates a new event with the given boolean topic determining whether it is active.
+   *
+   * @param loop the loop that polls this event
+   * @param inst The NetworkTable instance to use
+   * @param tableName The NetworkTable that contains the topic
+   * @param topicName The topic name within the table that contains the value
+   */
+  public NetworkBooleanEvent(
+      EventLoop loop, NetworkTableInstance inst, String tableName, String topicName) {
+    this(loop, inst.getTable(tableName), topicName);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/accelerometer/ADXL345_I2C.java

@@ -0,0 +1,285 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.DoubleTopic;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+/** ADXL345 I2C Accelerometer. */
+@SuppressWarnings("TypeName")
+public class ADXL345_I2C implements NTSendable, AutoCloseable {
+  /** Default I2C device address. */
+  public static final byte kAddress = 0x1D;
+
+  private static final byte kPowerCtlRegister = 0x2D;
+  private static final byte kDataFormatRegister = 0x31;
+  private static final byte kDataRegister = 0x32;
+  private static final double kGsPerLSB = 0.00390625;
+  // private static final byte kPowerCtl_Link = 0x20;
+  // private static final byte kPowerCtl_AutoSleep = 0x10;
+  private static final byte kPowerCtl_Measure = 0x08;
+  // private static final byte kPowerCtl_Sleep = 0x04;
+
+  // private static final byte kDataFormat_SelfTest = (byte) 0x80;
+  // private static final byte kDataFormat_SPI = 0x40;
+  // private static final byte kDataFormat_IntInvert = 0x20;
+  private static final byte kDataFormat_FullRes = 0x08;
+
+  // private static final byte kDataFormat_Justify = 0x04;
+
+  /** Accelerometer range. */
+  public enum Range {
+    /** 2 Gs max. */
+    k2G,
+    /** 4 Gs max. */
+    k4G,
+    /** 8 Gs max. */
+    k8G,
+    /** 16 Gs max. */
+    k16G
+  }
+
+  /** Accelerometer axes. */
+  public enum Axes {
+    /** X axis. */
+    kX((byte) 0x00),
+    /** Y axis. */
+    kY((byte) 0x02),
+    /** Z axis. */
+    kZ((byte) 0x04);
+
+    /** The integer value representing this enumeration. */
+    public final byte value;
+
+    Axes(byte value) {
+      this.value = value;
+    }
+  }
+
+  /** Container type for accelerations from all axes. */
+  @SuppressWarnings("MemberName")
+  public static class AllAxes {
+    /** Acceleration along the X axis in g-forces. */
+    public double XAxis;
+
+    /** Acceleration along the Y axis in g-forces. */
+    public double YAxis;
+
+    /** Acceleration along the Z axis in g-forces. */
+    public double ZAxis;
+
+    /** Default constructor. */
+    public AllAxes() {}
+  }
+
+  private I2C m_i2c;
+
+  private SimDevice m_simDevice;
+  private SimEnum m_simRange;
+  private SimDouble m_simX;
+  private SimDouble m_simY;
+  private SimDouble m_simZ;
+
+  /**
+   * Constructs the ADXL345 Accelerometer with I2C address 0x1D.
+   *
+   * @param port The I2C port the accelerometer is attached to
+   * @param range The range (+ or -) that the accelerometer will measure.
+   */
+  public ADXL345_I2C(I2C.Port port, Range range) {
+    this(port, range, kAddress);
+  }
+
+  /**
+   * Constructs the ADXL345 Accelerometer over I2C.
+   *
+   * @param port The I2C port the accelerometer is attached to
+   * @param range The range (+ or -) that the accelerometer will measure.
+   * @param deviceAddress I2C address of the accelerometer (0x1D or 0x53)
+   */
+  @SuppressWarnings("this-escape")
+  public ADXL345_I2C(I2C.Port port, Range range, int deviceAddress) {
+    m_i2c = new I2C(port, deviceAddress);
+
+    // simulation
+    m_simDevice = SimDevice.create("Accel:ADXL345_I2C", port.value, deviceAddress);
+    if (m_simDevice != null) {
+      m_simRange =
+          m_simDevice.createEnumDouble(
+              "range",
+              SimDevice.Direction.kOutput,
+              new String[] {"2G", "4G", "8G", "16G"},
+              new double[] {2.0, 4.0, 8.0, 16.0},
+              0);
+      m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0);
+      m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0);
+      m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0);
+    }
+
+    // Turn on the measurements
+    m_i2c.write(kPowerCtlRegister, kPowerCtl_Measure);
+
+    setRange(range);
+
+    HAL.reportUsage("I2C[" + port.value + "][" + deviceAddress + "]", "ADXL345");
+    SendableRegistry.add(this, "ADXL345_I2C", port.value);
+  }
+
+  /**
+   * Returns the I2C port.
+   *
+   * @return The I2C port.
+   */
+  public int getPort() {
+    return m_i2c.getPort();
+  }
+
+  /**
+   * Returns the I2C device address.
+   *
+   * @return The I2C device address.
+   */
+  public int getDeviceAddress() {
+    return m_i2c.getDeviceAddress();
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    if (m_i2c != null) {
+      m_i2c.close();
+      m_i2c = null;
+    }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+    }
+  }
+
+  /**
+   * Set the measuring range of the accelerometer.
+   *
+   * @param range The maximum acceleration, positive or negative, that the accelerometer will
+   *     measure.
+   */
+  public final void setRange(Range range) {
+    final byte value =
+        switch (range) {
+          case k2G -> 0;
+          case k4G -> 1;
+          case k8G -> 2;
+          case k16G -> 3;
+        };
+
+    // Specify the data format to read
+    m_i2c.write(kDataFormatRegister, kDataFormat_FullRes | value);
+
+    if (m_simRange != null) {
+      m_simRange.set(value);
+    }
+  }
+
+  /**
+   * Returns the acceleration along the X axis in g-forces.
+   *
+   * @return The acceleration along the X axis in g-forces.
+   */
+  public double getX() {
+    return getAcceleration(Axes.kX);
+  }
+
+  /**
+   * Returns the acceleration along the Y axis in g-forces.
+   *
+   * @return The acceleration along the Y axis in g-forces.
+   */
+  public double getY() {
+    return getAcceleration(Axes.kY);
+  }
+
+  /**
+   * Returns the acceleration along the Z axis in g-forces.
+   *
+   * @return The acceleration along the Z axis in g-forces.
+   */
+  public double getZ() {
+    return getAcceleration(Axes.kZ);
+  }
+
+  /**
+   * Get the acceleration of one axis in Gs.
+   *
+   * @param axis The axis to read from.
+   * @return Acceleration of the ADXL345 in Gs.
+   */
+  public double getAcceleration(Axes axis) {
+    if (axis == Axes.kX && m_simX != null) {
+      return m_simX.get();
+    }
+    if (axis == Axes.kY && m_simY != null) {
+      return m_simY.get();
+    }
+    if (axis == Axes.kZ && m_simZ != null) {
+      return m_simZ.get();
+    }
+    ByteBuffer rawAccel = ByteBuffer.allocate(2);
+    m_i2c.read(kDataRegister + axis.value, 2, rawAccel);
+
+    // Sensor is little endian... swap bytes
+    rawAccel.order(ByteOrder.LITTLE_ENDIAN);
+    return rawAccel.getShort(0) * kGsPerLSB;
+  }
+
+  /**
+   * Get the acceleration of all axes in Gs.
+   *
+   * @return An object containing the acceleration measured on each axis of the ADXL345 in Gs.
+   */
+  public AllAxes getAccelerations() {
+    AllAxes data = new AllAxes();
+    if (m_simX != null && m_simY != null && m_simZ != null) {
+      data.XAxis = m_simX.get();
+      data.YAxis = m_simY.get();
+      data.ZAxis = m_simZ.get();
+      return data;
+    }
+    ByteBuffer rawData = ByteBuffer.allocate(6);
+    m_i2c.read(kDataRegister, 6, rawData);
+
+    // Sensor is little endian... swap bytes
+    rawData.order(ByteOrder.LITTLE_ENDIAN);
+    data.XAxis = rawData.getShort(0) * kGsPerLSB;
+    data.YAxis = rawData.getShort(2) * kGsPerLSB;
+    data.ZAxis = rawData.getShort(4) * kGsPerLSB;
+    return data;
+  }
+
+  @Override
+  public void initSendable(NTSendableBuilder builder) {
+    builder.setSmartDashboardType("3AxisAccelerometer");
+    DoublePublisher pubX = new DoubleTopic(builder.getTopic("X")).publish();
+    DoublePublisher pubY = new DoubleTopic(builder.getTopic("Y")).publish();
+    DoublePublisher pubZ = new DoubleTopic(builder.getTopic("Z")).publish();
+    builder.addCloseable(pubX);
+    builder.addCloseable(pubY);
+    builder.addCloseable(pubZ);
+    builder.setUpdateTable(
+        () -> {
+          AllAxes data = getAccelerations();
+          pubX.set(data.XAxis);
+          pubY.set(data.YAxis);
+          pubZ.set(data.ZAxis);
+        });
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/accelerometer/AnalogAccelerometer.java

@@ -0,0 +1,117 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Handle operation of an analog accelerometer. The accelerometer reads acceleration directly
+ * through the sensor. Many sensors have multiple axis and can be treated as multiple devices. Each
+ * is calibrated by finding the center value over a period of time.
+ */
+public class AnalogAccelerometer implements Sendable, AutoCloseable {
+  private AnalogInput m_analogChannel;
+  private double m_voltsPerG = 1.0;
+  private double m_zeroGVoltage = 2.5;
+  private final boolean m_allocatedChannel;
+
+  /** Common initialization. */
+  private void initAccelerometer() {
+    HAL.reportUsage("IO", m_analogChannel.getChannel(), "Accelerometer");
+    SendableRegistry.add(this, "Accelerometer", m_analogChannel.getChannel());
+  }
+
+  /**
+   * Create a new instance of an accelerometer.
+   *
+   * <p>The constructor allocates desired analog channel.
+   *
+   * @param channel The channel number for the analog input the accelerometer is connected to
+   */
+  @SuppressWarnings("this-escape")
+  public AnalogAccelerometer(final int channel) {
+    this(new AnalogInput(channel), true);
+    SendableRegistry.addChild(this, m_analogChannel);
+  }
+
+  /**
+   * Create a new instance of Accelerometer from an existing AnalogChannel. Make a new instance of
+   * accelerometer given an AnalogChannel. This is particularly useful if the port is going to be
+   * read as an analog channel as well as through the Accelerometer class.
+   *
+   * @param channel The existing AnalogInput object for the analog input the accelerometer is
+   *     connected to
+   */
+  @SuppressWarnings("this-escape")
+  public AnalogAccelerometer(final AnalogInput channel) {
+    this(channel, false);
+  }
+
+  @SuppressWarnings("this-escape")
+  private AnalogAccelerometer(final AnalogInput channel, final boolean allocatedChannel) {
+    requireNonNullParam(channel, "channel", "AnalogAccelerometer");
+    m_allocatedChannel = allocatedChannel;
+    m_analogChannel = channel;
+    initAccelerometer();
+  }
+
+  /** Delete the analog components used for the accelerometer. */
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    if (m_analogChannel != null && m_allocatedChannel) {
+      m_analogChannel.close();
+    }
+    m_analogChannel = null;
+  }
+
+  /**
+   * Return the acceleration in Gs.
+   *
+   * <p>The acceleration is returned units of Gs.
+   *
+   * @return The current acceleration of the sensor in Gs.
+   */
+  public double getAcceleration() {
+    if (m_analogChannel == null) {
+      return 0.0;
+    }
+    return (m_analogChannel.getVoltage() - m_zeroGVoltage) / m_voltsPerG;
+  }
+
+  /**
+   * Set the accelerometer sensitivity.
+   *
+   * <p>This sets the sensitivity of the accelerometer used for calculating the acceleration. The
+   * sensitivity varies by accelerometer model.
+   *
+   * @param sensitivity The sensitivity of accelerometer in Volts per G.
+   */
+  public void setSensitivity(double sensitivity) {
+    m_voltsPerG = sensitivity;
+  }
+
+  /**
+   * Set the voltage that corresponds to 0 G.
+   *
+   * <p>The zero G voltage varies by accelerometer model.
+   *
+   * @param zero The zero G voltage.
+   */
+  public void setZero(double zero) {
+    m_zeroGVoltage = zero;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Accelerometer");
+    builder.addDoubleProperty("Value", this::getAcceleration, null);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/bus/CAN.java

@@ -0,0 +1,194 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.CANAPIJNI;
+import edu.wpi.first.hal.CANAPITypes;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.can.CANReceiveMessage;
+import java.io.Closeable;
+
+/**
+ * High level class for interfacing with CAN devices conforming to the standard CAN spec.
+ *
+ * <p>No packets that can be sent gets blocked by the RoboRIO, so all methods work identically in
+ * all robot modes.
+ *
+ * <p>All methods are thread safe, however the CANData object passed into the read methods and the
+ * byte[] passed into the write methods need to not be modified for the duration of their respective
+ * calls.
+ */
+public class CAN implements Closeable {
+  /** Team manufacturer. */
+  public static final int kTeamManufacturer = CANAPITypes.CANManufacturer.kTeamUse.id;
+
+  /** Team device type. */
+  public static final int kTeamDeviceType = CANAPITypes.CANDeviceType.kMiscellaneous.id;
+
+  private final int m_handle;
+
+  /**
+   * Create a new CAN communication interface with the specific device ID. This uses the team
+   * manufacturer and device types. The device ID is 6 bits (0-63).
+   *
+   * @param busId The bus ID
+   * @param deviceId The device id
+   */
+  public CAN(int busId, int deviceId) {
+    this(busId, deviceId, kTeamManufacturer, kTeamDeviceType);
+  }
+
+  /**
+   * Create a new CAN communication interface with a specific device ID, manufacturer and device
+   * type. The device ID is 6 bits, the manufacturer is 8 bits, and the device type is 5 bits.
+   *
+   * @param busId The bus ID
+   * @param deviceId The device ID
+   * @param deviceManufacturer The device manufacturer
+   * @param deviceType The device type
+   */
+  public CAN(int busId, int deviceId, int deviceManufacturer, int deviceType) {
+    m_handle = CANAPIJNI.initializeCAN(busId, deviceManufacturer, deviceId, deviceType);
+    HAL.reportUsage("CAN[" + deviceType + "][" + deviceManufacturer + "][" + deviceId + "]", "");
+  }
+
+  /** Closes the CAN communication. */
+  @Override
+  public void close() {
+    if (m_handle != 0) {
+      CANAPIJNI.cleanCAN(m_handle);
+    }
+  }
+
+  /**
+   * Write a packet to the CAN device with a specific ID. This ID is 10 bits.
+   *
+   * @param apiId The API ID to write.
+   * @param data The data to write
+   * @param dataLength The data length
+   * @param flags The flags
+   */
+  public void writePacket(int apiId, byte[] data, int dataLength, int flags) {
+    CANAPIJNI.writeCANPacket(m_handle, apiId, data, dataLength, flags);
+  }
+
+  /**
+   * Write a repeating packet to the CAN device with a specific ID. This ID is 10 bits. The RoboRIO
+   * will automatically repeat the packet at the specified interval
+   *
+   * @param apiId The API ID to write.
+   * @param data The data to write
+   * @param dataLength The data length
+   * @param flags The flags
+   * @param repeatMs The period to repeat the packet at.
+   */
+  public void writePacketRepeating(
+      int apiId, byte[] data, int dataLength, int flags, int repeatMs) {
+    CANAPIJNI.writeCANPacketRepeating(m_handle, apiId, data, dataLength, flags, repeatMs);
+  }
+
+  /**
+   * Write an RTR frame to the CAN device with a specific ID. This ID is 10 bits. The length by spec
+   * must match what is returned by the responding device
+   *
+   * @param apiId The API ID to write.
+   * @param data The data to write
+   * @param dataLength The data length
+   * @param flags The flags
+   */
+  public void writeRTRFrame(int apiId, byte[] data, int dataLength, int flags) {
+    CANAPIJNI.writeCANRTRFrame(m_handle, apiId, data, dataLength, flags);
+  }
+
+  /**
+   * Write a packet to the CAN device with a specific ID. This ID is 10 bits.
+   *
+   * @param apiId The API ID to write.
+   * @param data The data to write
+   * @param dataLength The data length
+   * @param flags The flags
+   * @return TODO
+   */
+  public int writePacketNoThrow(int apiId, byte[] data, int dataLength, int flags) {
+    return CANAPIJNI.writeCANPacketNoThrow(m_handle, apiId, data, dataLength, flags);
+  }
+
+  /**
+   * Write a repeating packet to the CAN device with a specific ID. This ID is 10 bits. The RoboRIO
+   * will automatically repeat the packet at the specified interval
+   *
+   * @param apiId The API ID to write.
+   * @param data The data to write
+   * @param dataLength The data length
+   * @param flags The flags
+   * @param repeatMs The period to repeat the packet at.
+   * @return TODO
+   */
+  public int writePacketRepeatingNoThrow(
+      int apiId, byte[] data, int dataLength, int flags, int repeatMs) {
+    return CANAPIJNI.writeCANPacketRepeatingNoThrow(
+        m_handle, apiId, data, dataLength, flags, repeatMs);
+  }
+
+  /**
+   * Write an RTR frame to the CAN device with a specific ID. This ID is 10 bits. The length by spec
+   * must match what is returned by the responding device
+   *
+   * @param apiId The API ID to write.
+   * @param data The data to write
+   * @param dataLength The data length
+   * @param flags The flags
+   * @return TODO
+   */
+  public int writeRTRFrameNoThrow(int apiId, byte[] data, int dataLength, int flags) {
+    return CANAPIJNI.writeCANRTRFrameNoThrow(m_handle, apiId, data, dataLength, flags);
+  }
+
+  /**
+   * Stop a repeating packet with a specific ID. This ID is 10 bits.
+   *
+   * @param apiId The API ID to stop repeating
+   */
+  public void stopPacketRepeating(int apiId) {
+    CANAPIJNI.stopCANPacketRepeating(m_handle, apiId);
+  }
+
+  /**
+   * Read a new CAN packet. This will only return properly once per packet received. Multiple calls
+   * without receiving another packet will return false.
+   *
+   * @param apiId The API ID to read.
+   * @param data Storage for the received data.
+   * @return True if the data is valid, otherwise false.
+   */
+  public boolean readPacketNew(int apiId, CANReceiveMessage data) {
+    return CANAPIJNI.readCANPacketNew(m_handle, apiId, data);
+  }
+
+  /**
+   * Read a CAN packet. This will continuously return the last packet received, without accounting
+   * for packet age.
+   *
+   * @param apiId The API ID to read.
+   * @param data Storage for the received data.
+   * @return True if the data is valid, otherwise false.
+   */
+  public boolean readPacketLatest(int apiId, CANReceiveMessage data) {
+    return CANAPIJNI.readCANPacketLatest(m_handle, apiId, data);
+  }
+
+  /**
+   * Read a CAN packet. This will return the last packet received until the packet is older than the
+   * requested timeout. Then it will return false.
+   *
+   * @param apiId The API ID to read.
+   * @param timeoutMs The timeout time for the packet
+   * @param data Storage for the received data.
+   * @return True if the data is valid, otherwise false.
+   */
+  public boolean readPacketTimeout(int apiId, CANReceiveMessage data, int timeoutMs) {
+    return CANAPIJNI.readCANPacketTimeout(m_handle, apiId, data, timeoutMs);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/bus/I2C.java

@@ -0,0 +1,390 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.I2CJNI;
+import edu.wpi.first.hal.util.BoundaryException;
+import java.nio.ByteBuffer;
+
+/**
+ * I2C bus interface class.
+ *
+ * <p>This class is intended to be used by sensor (and other I2C device) drivers. It probably should
+ * not be used directly.
+ */
+public class I2C implements AutoCloseable {
+  /** I2C connection ports. */
+  public enum Port {
+    /** I2C Port 0. */
+    kPort0(0),
+    /** I2C Port 1. */
+    kPort1(1);
+
+    /** Port value. */
+    public final int value;
+
+    Port(int value) {
+      this.value = value;
+    }
+  }
+
+  private final int m_port;
+  private final int m_deviceAddress;
+  private ByteBuffer m_readDataToSendBuffer;
+
+  /**
+   * Constructor.
+   *
+   * @param port The I2C port the device is connected to.
+   * @param deviceAddress The address of the device on the I2C bus.
+   */
+  public I2C(Port port, int deviceAddress) {
+    m_port = port.value;
+    m_deviceAddress = deviceAddress;
+
+    I2CJNI.i2CInitialize((byte) port.value);
+
+    HAL.reportUsage("I2C[" + port.value + "][" + deviceAddress + "]", "");
+  }
+
+  /**
+   * Returns I2C port.
+   *
+   * @return I2C port.
+   */
+  public int getPort() {
+    return m_port;
+  }
+
+  /**
+   * Returns I2C device address.
+   *
+   * @return I2C device address.
+   */
+  public int getDeviceAddress() {
+    return m_deviceAddress;
+  }
+
+  @Override
+  public void close() {
+    I2CJNI.i2CClose(m_port);
+  }
+
+  /**
+   * Generic transaction.
+   *
+   * <p>This is a lower-level interface to the I2C hardware giving you more control over each
+   * transaction. If you intend to write multiple bytes in the same transaction and do not plan to
+   * receive anything back, use writeBulk() instead. Calling this with a receiveSize of 0 will
+   * result in an error.
+   *
+   * @param dataToSend Buffer of data to send as part of the transaction.
+   * @param sendSize Number of bytes to send as part of the transaction.
+   * @param dataReceived Buffer to read data into.
+   * @param receiveSize Number of bytes to read from the device.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  public synchronized boolean transaction(
+      byte[] dataToSend, int sendSize, byte[] dataReceived, int receiveSize) {
+    if (dataToSend.length < sendSize) {
+      throw new IllegalArgumentException("dataToSend is too small, must be at least " + sendSize);
+    }
+    if (dataReceived.length < receiveSize) {
+      throw new IllegalArgumentException(
+          "dataReceived is too small, must be at least " + receiveSize);
+    }
+    return I2CJNI.i2CTransactionB(
+            m_port,
+            (byte) m_deviceAddress,
+            dataToSend,
+            (byte) sendSize,
+            dataReceived,
+            (byte) receiveSize)
+        < 0;
+  }
+
+  /**
+   * Generic transaction.
+   *
+   * <p>This is a lower-level interface to the I2C hardware giving you more control over each
+   * transaction.
+   *
+   * @param dataToSend Buffer of data to send as part of the transaction.
+   * @param sendSize Number of bytes to send as part of the transaction.
+   * @param dataReceived Buffer to read data into.
+   * @param receiveSize Number of bytes to read from the device.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  public synchronized boolean transaction(
+      ByteBuffer dataToSend, int sendSize, ByteBuffer dataReceived, int receiveSize) {
+    if (dataToSend.hasArray() && dataReceived.hasArray()) {
+      return transaction(dataToSend.array(), sendSize, dataReceived.array(), receiveSize);
+    }
+    if (!dataToSend.isDirect()) {
+      throw new IllegalArgumentException("dataToSend must be a direct buffer");
+    }
+    if (dataToSend.capacity() < sendSize) {
+      throw new IllegalArgumentException("dataToSend is too small, must be at least " + sendSize);
+    }
+    if (!dataReceived.isDirect()) {
+      throw new IllegalArgumentException("dataReceived must be a direct buffer");
+    }
+    if (dataReceived.capacity() < receiveSize) {
+      throw new IllegalArgumentException(
+          "dataReceived is too small, must be at least " + receiveSize);
+    }
+
+    return I2CJNI.i2CTransaction(
+            m_port,
+            (byte) m_deviceAddress,
+            dataToSend,
+            (byte) sendSize,
+            dataReceived,
+            (byte) receiveSize)
+        < 0;
+  }
+
+  /**
+   * Attempt to address a device on the I2C bus.
+   *
+   * <p>This allows you to figure out if there is a device on the I2C bus that responds to the
+   * address specified in the constructor.
+   *
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  public boolean addressOnly() {
+    return transaction(new byte[0], (byte) 0, new byte[0], (byte) 0);
+  }
+
+  /**
+   * Execute a write transaction with the device.
+   *
+   * <p>Write a single byte to a register on a device and wait until the transaction is complete.
+   *
+   * @param registerAddress The address of the register on the device to be written.
+   * @param data The byte to write to the register on the device.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  public synchronized boolean write(int registerAddress, int data) {
+    byte[] buffer = new byte[2];
+    buffer[0] = (byte) registerAddress;
+    buffer[1] = (byte) data;
+    return I2CJNI.i2CWriteB(m_port, (byte) m_deviceAddress, buffer, (byte) buffer.length) < 0;
+  }
+
+  /**
+   * Execute a write transaction with the device.
+   *
+   * <p>Write multiple bytes to a register on a device and wait until the transaction is complete.
+   *
+   * @param data The data to write to the device.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  public synchronized boolean writeBulk(byte[] data) {
+    return writeBulk(data, data.length);
+  }
+
+  /**
+   * Execute a write transaction with the device.
+   *
+   * <p>Write multiple bytes to a register on a device and wait until the transaction is complete.
+   *
+   * @param data The data to write to the device.
+   * @param size The number of data bytes to write.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  public synchronized boolean writeBulk(byte[] data, int size) {
+    if (data.length < size) {
+      throw new IllegalArgumentException("buffer is too small, must be at least " + size);
+    }
+    return I2CJNI.i2CWriteB(m_port, (byte) m_deviceAddress, data, (byte) size) < 0;
+  }
+
+  /**
+   * Execute a write transaction with the device.
+   *
+   * <p>Write multiple bytes to a register on a device and wait until the transaction is complete.
+   *
+   * @param data The data to write to the device.
+   * @param size The number of data bytes to write.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  public synchronized boolean writeBulk(ByteBuffer data, int size) {
+    if (data.hasArray()) {
+      return writeBulk(data.array(), size);
+    }
+    if (!data.isDirect()) {
+      throw new IllegalArgumentException("must be a direct buffer");
+    }
+    if (data.capacity() < size) {
+      throw new IllegalArgumentException("buffer is too small, must be at least " + size);
+    }
+
+    return I2CJNI.i2CWrite(m_port, (byte) m_deviceAddress, data, (byte) size) < 0;
+  }
+
+  /**
+   * Execute a read transaction with the device.
+   *
+   * <p>Read bytes from a device. Most I2C devices will auto-increment the register pointer
+   * internally allowing you to read consecutive registers on a device in a single transaction.
+   *
+   * @param registerAddress The register to read first in the transaction.
+   * @param count The number of bytes to read in the transaction.
+   * @param buffer A pointer to the array of bytes to store the data read from the device.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  public boolean read(int registerAddress, int count, byte[] buffer) {
+    requireNonNullParam(buffer, "buffer", "read");
+
+    if (count < 1) {
+      throw new BoundaryException("Value must be at least 1, " + count + " given");
+    }
+    if (buffer.length < count) {
+      throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+    }
+
+    byte[] registerAddressArray = new byte[1];
+    registerAddressArray[0] = (byte) registerAddress;
+
+    return transaction(registerAddressArray, registerAddressArray.length, buffer, count);
+  }
+
+  /**
+   * Execute a read transaction with the device.
+   *
+   * <p>Read bytes from a device. Most I2C devices will auto-increment the register pointer
+   * internally allowing you to read consecutive registers on a device in a single transaction.
+   *
+   * @param registerAddress The register to read first in the transaction.
+   * @param count The number of bytes to read in the transaction.
+   * @param buffer A buffer to store the data read from the device.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  public boolean read(int registerAddress, int count, ByteBuffer buffer) {
+    if (count < 1) {
+      throw new BoundaryException("Value must be at least 1, " + count + " given");
+    }
+
+    if (buffer.hasArray()) {
+      return read(registerAddress, count, buffer.array());
+    }
+
+    if (!buffer.isDirect()) {
+      throw new IllegalArgumentException("must be a direct buffer");
+    }
+    if (buffer.capacity() < count) {
+      throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+    }
+
+    synchronized (this) {
+      if (m_readDataToSendBuffer == null) {
+        m_readDataToSendBuffer = ByteBuffer.allocateDirect(1);
+      }
+      m_readDataToSendBuffer.put(0, (byte) registerAddress);
+
+      return transaction(m_readDataToSendBuffer, 1, buffer, count);
+    }
+  }
+
+  /**
+   * Execute a read only transaction with the device.
+   *
+   * <p>Read bytes from a device. This method does not write any data to prompt the device.
+   *
+   * @param buffer A pointer to the array of bytes to store the data read from the device.
+   * @param count The number of bytes to read in the transaction.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  public boolean readOnly(byte[] buffer, int count) {
+    requireNonNullParam(buffer, "buffer", "readOnly");
+    if (count < 1) {
+      throw new BoundaryException("Value must be at least 1, " + count + " given");
+    }
+    if (buffer.length < count) {
+      throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+    }
+
+    return I2CJNI.i2CReadB(m_port, (byte) m_deviceAddress, buffer, (byte) count) < 0;
+  }
+
+  /**
+   * Execute a read only transaction with the device.
+   *
+   * <p>Read bytes from a device. This method does not write any data to prompt the device.
+   *
+   * @param buffer A pointer to the array of bytes to store the data read from the device.
+   * @param count The number of bytes to read in the transaction.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  public boolean readOnly(ByteBuffer buffer, int count) {
+    if (count < 1) {
+      throw new BoundaryException("Value must be at least 1, " + count + " given");
+    }
+
+    if (buffer.hasArray()) {
+      return readOnly(buffer.array(), count);
+    }
+
+    if (!buffer.isDirect()) {
+      throw new IllegalArgumentException("must be a direct buffer");
+    }
+    if (buffer.capacity() < count) {
+      throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+    }
+
+    return I2CJNI.i2CRead(m_port, (byte) m_deviceAddress, buffer, (byte) count) < 0;
+  }
+
+  /*
+   * Send a broadcast write to all devices on the I2C bus.
+   *
+   * <p>This is not currently implemented!
+   *
+   * @param registerAddress The register to write on all devices on the bus.
+   * @param data            The value to write to the devices.
+   */
+  // public void broadcast(int registerAddress, int data) {
+  // }
+
+  /**
+   * Verify that a device's registers contain expected values.
+   *
+   * <p>Most devices will have a set of registers that contain a known value that can be used to
+   * identify them. This allows an I2C device driver to easily verify that the device contains the
+   * expected value.
+   *
+   * @param registerAddress The base register to start reading from the device.
+   * @param count The size of the field to be verified.
+   * @param expected A buffer containing the values expected from the device.
+   * @return true if the sensor was verified to be connected
+   * @pre The device must support and be configured to use register auto-increment.
+   */
+  public boolean verifySensor(int registerAddress, int count, byte[] expected) {
+    // TODO: Make use of all 7 read bytes
+    byte[] dataToSend = new byte[1];
+
+    byte[] deviceData = new byte[4];
+    for (int i = 0; i < count; i += 4) {
+      int toRead = Math.min(count - i, 4);
+      // Read the chunk of data. Return false if the sensor does not
+      // respond.
+      dataToSend[0] = (byte) (registerAddress + i);
+      if (transaction(dataToSend, 1, deviceData, toRead)) {
+        return false;
+      }
+
+      for (byte j = 0; j < toRead; j++) {
+        if (deviceData[j] != expected[i + j]) {
+          return false;
+        }
+      }
+    }
+    return true;
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/bus/SerialPort.java

@@ -0,0 +1,358 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SerialPortJNI;
+import java.nio.charset.StandardCharsets;
+
+/** Driver for the serial ports (USB, MXP, Onboard) on the roboRIO. */
+public class SerialPort implements AutoCloseable {
+  private int m_portHandle;
+
+  /** Serial port. */
+  public enum Port {
+    /** Onboard serial port on the roboRIO. */
+    kOnboard(0),
+    /** MXP (roboRIO MXP) serial port. */
+    kMXP(1),
+    /** USB serial port (same as kUSB1). */
+    kUSB(2),
+    /** USB serial port 1. */
+    kUSB1(2),
+    /** USB serial port 2. */
+    kUSB2(3);
+
+    /** Port value. */
+    public final int value;
+
+    Port(int value) {
+      this.value = value;
+    }
+  }
+
+  /** Represents the parity to use for serial communications. */
+  public enum Parity {
+    /** No parity. */
+    kNone(0),
+    /** Odd parity. */
+    kOdd(1),
+    /** Even parity. */
+    kEven(2),
+    /** Parity bit always on. */
+    kMark(3),
+    /** Parity bit always off. */
+    kSpace(4);
+
+    /** Parity value. */
+    public final int value;
+
+    Parity(int value) {
+      this.value = value;
+    }
+  }
+
+  /** Represents the number of stop bits to use for Serial Communication. */
+  public enum StopBits {
+    /** One stop bit. */
+    kOne(10),
+    /** One and a half stop bits. */
+    kOnePointFive(15),
+    /** Two stop bits. */
+    kTwo(20);
+
+    /** StopBits value. */
+    public final int value;
+
+    StopBits(int value) {
+      this.value = value;
+    }
+  }
+
+  /** Represents what type of flow control to use for serial communication. */
+  public enum FlowControl {
+    /** No flow control. */
+    kNone(0),
+    /** XON/XOFF flow control. */
+    kXonXoff(1),
+    /** RTS/CTS flow control. */
+    kRtsCts(2),
+    /** DTS/DSR flow control. */
+    kDtsDsr(4);
+
+    /** FlowControl value. */
+    public final int value;
+
+    FlowControl(int value) {
+      this.value = value;
+    }
+  }
+
+  /** Represents which type of buffer mode to use when writing to a serial port. */
+  public enum WriteBufferMode {
+    /** Flush the buffer on each access. */
+    kFlushOnAccess(1),
+    /** Flush the buffer when it is full. */
+    kFlushWhenFull(2);
+
+    /** WriteBufferMode value. */
+    public final int value;
+
+    WriteBufferMode(int value) {
+      this.value = value;
+    }
+  }
+
+  /**
+   * Create an instance of a Serial Port class.
+   *
+   * @param baudRate The baud rate to configure the serial port.
+   * @param port The Serial port to use
+   * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+   * @param parity Select the type of parity checking to use.
+   * @param stopBits The number of stop bits to use as defined by the enum StopBits.
+   */
+  public SerialPort(
+      final int baudRate, Port port, final int dataBits, Parity parity, StopBits stopBits) {
+    m_portHandle = SerialPortJNI.serialInitializePort((byte) port.value);
+    SerialPortJNI.serialSetBaudRate(m_portHandle, baudRate);
+    SerialPortJNI.serialSetDataBits(m_portHandle, (byte) dataBits);
+    SerialPortJNI.serialSetParity(m_portHandle, (byte) parity.value);
+    SerialPortJNI.serialSetStopBits(m_portHandle, (byte) stopBits.value);
+
+    // Set the default read buffer size to 1 to return bytes immediately
+    setReadBufferSize(1);
+
+    // Set the default timeout to 5 seconds.
+    setTimeout(5.0);
+
+    // Don't wait until the buffer is full to transmit.
+    setWriteBufferMode(WriteBufferMode.kFlushOnAccess);
+
+    disableTermination();
+
+    HAL.reportUsage("SerialPort", port.value, "");
+  }
+
+  /**
+   * Create an instance of a Serial Port class. Defaults to one stop bit.
+   *
+   * @param baudRate The baud rate to configure the serial port.
+   * @param port The serial port to use.
+   * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+   * @param parity Select the type of parity checking to use.
+   */
+  public SerialPort(final int baudRate, Port port, final int dataBits, Parity parity) {
+    this(baudRate, port, dataBits, parity, StopBits.kOne);
+  }
+
+  /**
+   * Create an instance of a Serial Port class. Defaults to no parity and one stop bit.
+   *
+   * @param baudRate The baud rate to configure the serial port.
+   * @param port The serial port to use.
+   * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+   */
+  public SerialPort(final int baudRate, Port port, final int dataBits) {
+    this(baudRate, port, dataBits, Parity.kNone, StopBits.kOne);
+  }
+
+  /**
+   * Create an instance of a Serial Port class. Defaults to 8 databits, no parity, and one stop bit.
+   *
+   * @param baudRate The baud rate to configure the serial port.
+   * @param port The serial port to use.
+   */
+  public SerialPort(final int baudRate, Port port) {
+    this(baudRate, port, 8, Parity.kNone, StopBits.kOne);
+  }
+
+  @Override
+  public void close() {
+    SerialPortJNI.serialClose(m_portHandle);
+  }
+
+  /**
+   * Set the type of flow control to enable on this port.
+   *
+   * <p>By default, flow control is disabled.
+   *
+   * @param flowControl the FlowControl m_value to use
+   */
+  public void setFlowControl(FlowControl flowControl) {
+    SerialPortJNI.serialSetFlowControl(m_portHandle, (byte) flowControl.value);
+  }
+
+  /**
+   * Enable termination and specify the termination character.
+   *
+   * <p>Termination is currently only implemented for receive. When the terminator is received, the
+   * read() or readString() will return fewer bytes than requested, stopping after the terminator.
+   *
+   * @param terminator The character to use for termination.
+   */
+  public void enableTermination(char terminator) {
+    SerialPortJNI.serialEnableTermination(m_portHandle, terminator);
+  }
+
+  /**
+   * Enable termination with the default terminator '\n'
+   *
+   * <p>Termination is currently only implemented for receive. When the terminator is received, the
+   * read() or readString() will return fewer bytes than requested, stopping after the terminator.
+   *
+   * <p>The default terminator is '\n'
+   */
+  public void enableTermination() {
+    enableTermination('\n');
+  }
+
+  /** Disable termination behavior. */
+  public final void disableTermination() {
+    SerialPortJNI.serialDisableTermination(m_portHandle);
+  }
+
+  /**
+   * Get the number of bytes currently available to read from the serial port.
+   *
+   * @return The number of bytes available to read.
+   */
+  public int getBytesReceived() {
+    return SerialPortJNI.serialGetBytesReceived(m_portHandle);
+  }
+
+  /**
+   * Read a string out of the buffer. Reads the entire contents of the buffer
+   *
+   * @return The read string
+   */
+  public String readString() {
+    return readString(getBytesReceived());
+  }
+
+  /**
+   * Read a string out of the buffer. Reads the entire contents of the buffer
+   *
+   * @param count the number of characters to read into the string
+   * @return The read string
+   */
+  public String readString(int count) {
+    byte[] out = read(count);
+    return new String(out, StandardCharsets.US_ASCII);
+  }
+
+  /**
+   * Read raw bytes out of the buffer.
+   *
+   * @param count The maximum number of bytes to read.
+   * @return An array of the read bytes
+   */
+  public byte[] read(final int count) {
+    byte[] dataReceivedBuffer = new byte[count];
+    int gotten = SerialPortJNI.serialRead(m_portHandle, dataReceivedBuffer, count);
+    if (gotten == count) {
+      return dataReceivedBuffer;
+    }
+    byte[] retVal = new byte[gotten];
+    System.arraycopy(dataReceivedBuffer, 0, retVal, 0, gotten);
+    return retVal;
+  }
+
+  /**
+   * Write raw bytes to the serial port.
+   *
+   * @param buffer The buffer of bytes to write.
+   * @param count The maximum number of bytes to write.
+   * @return The number of bytes actually written into the port.
+   */
+  public int write(byte[] buffer, int count) {
+    if (buffer.length < count) {
+      throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+    }
+    return SerialPortJNI.serialWrite(m_portHandle, buffer, count);
+  }
+
+  /**
+   * Write a string to the serial port.
+   *
+   * @param data The string to write to the serial port.
+   * @return The number of bytes actually written into the port.
+   */
+  public int writeString(String data) {
+    return write(data.getBytes(StandardCharsets.UTF_8), data.length());
+  }
+
+  /**
+   * Configure the timeout of the serial m_port.
+   *
+   * <p>This defines the timeout for transactions with the hardware. It will affect reads if less
+   * bytes are available than the read buffer size (defaults to 1) and very large writes.
+   *
+   * @param timeout The number of seconds to wait for I/O.
+   */
+  public final void setTimeout(double timeout) {
+    SerialPortJNI.serialSetTimeout(m_portHandle, timeout);
+  }
+
+  /**
+   * Specify the size of the input buffer.
+   *
+   * <p>Specify the amount of data that can be stored before data from the device is returned to
+   * Read. If you want data that is received to be returned immediately, set this to 1.
+   *
+   * <p>It the buffer is not filled before the read timeout expires, all data that has been received
+   * so far will be returned.
+   *
+   * @param size The read buffer size.
+   */
+  public final void setReadBufferSize(int size) {
+    SerialPortJNI.serialSetReadBufferSize(m_portHandle, size);
+  }
+
+  /**
+   * Specify the size of the output buffer.
+   *
+   * <p>Specify the amount of data that can be stored before being transmitted to the device.
+   *
+   * @param size The write buffer size.
+   */
+  public void setWriteBufferSize(int size) {
+    SerialPortJNI.serialSetWriteBufferSize(m_portHandle, size);
+  }
+
+  /**
+   * Specify the flushing behavior of the output buffer.
+   *
+   * <p>When set to kFlushOnAccess, data is synchronously written to the serial port after each call
+   * to either print() or write().
+   *
+   * <p>When set to kFlushWhenFull, data will only be written to the serial port when the buffer is
+   * full or when flush() is called.
+   *
+   * @param mode The write buffer mode.
+   */
+  public final void setWriteBufferMode(WriteBufferMode mode) {
+    SerialPortJNI.serialSetWriteMode(m_portHandle, (byte) mode.value);
+  }
+
+  /**
+   * Force the output buffer to be written to the port.
+   *
+   * <p>This is used when setWriteBufferMode() is set to kFlushWhenFull to force a flush before the
+   * buffer is full.
+   */
+  public void flush() {
+    SerialPortJNI.serialFlush(m_portHandle);
+  }
+
+  /**
+   * Reset the serial port driver to a known state.
+   *
+   * <p>Empty the transmit and receive buffers in the device and formatted I/O.
+   */
+  public void reset() {
+    SerialPortJNI.serialClear(m_portHandle);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/discrete/AnalogInput.java

@@ -0,0 +1,213 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Analog channel class.
+ *
+ * <p>Each analog channel is read from hardware as a 12-bit number representing 0V to 3.3V.
+ *
+ * <p>Connected to each analog channel is an averaging and oversampling engine. This engine
+ * accumulates the specified ( by setAverageBits() and setOversampleBits() ) number of samples
+ * before returning a new value. This is not a sliding window average. The only difference between
+ * the oversampled samples and the averaged samples is that the oversampled samples are simply
+ * accumulated effectively increasing the resolution, while the averaged samples are divided by the
+ * number of samples to retain the resolution, but get more stable values.
+ */
+public class AnalogInput implements Sendable, AutoCloseable {
+  int m_port; // explicit no modifier, private and package accessible.
+  private int m_channel;
+
+  /**
+   * Construct an analog channel.
+   *
+   * @param channel The channel number to represent. 0-3 are on-board 4-7 are on the MXP port.
+   */
+  @SuppressWarnings("this-escape")
+  public AnalogInput(final int channel) {
+    AnalogJNI.checkAnalogInputChannel(channel);
+    m_channel = channel;
+
+    m_port = AnalogJNI.initializeAnalogInputPort(channel);
+
+    HAL.reportUsage("IO", channel, "AnalogInput");
+    SendableRegistry.add(this, "AnalogInput", channel);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    AnalogJNI.freeAnalogInputPort(m_port);
+    m_port = 0;
+    m_channel = 0;
+  }
+
+  /**
+   * Get a sample straight from this channel. The sample is a 12-bit value representing the 0V to
+   * 3.3V range of the A/D converter. The units are in A/D converter codes. Use GetVoltage() to get
+   * the analog value in calibrated units.
+   *
+   * @return A sample straight from this channel.
+   */
+  public int getValue() {
+    return AnalogJNI.getAnalogValue(m_port);
+  }
+
+  /**
+   * Get a sample from the output of the oversample and average engine for this channel. The sample
+   * is 12-bit + the bits configured in SetOversampleBits(). The value configured in
+   * setAverageBits() will cause this value to be averaged 2^bits number of samples. This is not a
+   * sliding window. The sample will not change until 2^(OversampleBits + AverageBits) samples have
+   * been acquired from this channel. Use getAverageVoltage() to get the analog value in calibrated
+   * units.
+   *
+   * @return A sample from the oversample and average engine for this channel.
+   */
+  public int getAverageValue() {
+    return AnalogJNI.getAnalogAverageValue(m_port);
+  }
+
+  /**
+   * Get a scaled sample straight from this channel. The value is scaled to units of Volts using the
+   * calibrated scaling data from getLSBWeight() and getOffset().
+   *
+   * @return A scaled sample straight from this channel.
+   */
+  public double getVoltage() {
+    return AnalogJNI.getAnalogVoltage(m_port);
+  }
+
+  /**
+   * Get a scaled sample from the output of the oversample and average engine for this channel. The
+   * value is scaled to units of Volts using the calibrated scaling data from getLSBWeight() and
+   * getOffset(). Using oversampling will cause this value to be higher resolution, but it will
+   * update more slowly. Using averaging will cause this value to be more stable, but it will update
+   * more slowly.
+   *
+   * @return A scaled sample from the output of the oversample and average engine for this channel.
+   */
+  public double getAverageVoltage() {
+    return AnalogJNI.getAnalogAverageVoltage(m_port);
+  }
+
+  /**
+   * Get the factory scaling the least significant bit weight constant. The least significant bit
+   * weight constant for the channel that was calibrated in manufacturing and stored in an eeprom.
+   *
+   * <p>Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+   *
+   * @return Least significant bit weight.
+   */
+  public long getLSBWeight() {
+    return AnalogJNI.getAnalogLSBWeight(m_port);
+  }
+
+  /**
+   * Get the factory scaling offset constant. The offset constant for the channel that was
+   * calibrated in manufacturing and stored in an eeprom.
+   *
+   * <p>Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+   *
+   * @return Offset constant.
+   */
+  public int getOffset() {
+    return AnalogJNI.getAnalogOffset(m_port);
+  }
+
+  /**
+   * Get the channel number.
+   *
+   * @return The channel number.
+   */
+  public int getChannel() {
+    return m_channel;
+  }
+
+  /**
+   * Set the number of averaging bits. This sets the number of averaging bits. The actual number of
+   * averaged samples is 2^bits. The averaging is done automatically in the FPGA.
+   *
+   * @param bits The number of averaging bits.
+   */
+  public void setAverageBits(final int bits) {
+    AnalogJNI.setAnalogAverageBits(m_port, bits);
+  }
+
+  /**
+   * Get the number of averaging bits. This gets the number of averaging bits from the FPGA. The
+   * actual number of averaged samples is 2^bits. The averaging is done automatically in the FPGA.
+   *
+   * @return The number of averaging bits.
+   */
+  public int getAverageBits() {
+    return AnalogJNI.getAnalogAverageBits(m_port);
+  }
+
+  /**
+   * Set the number of oversample bits. This sets the number of oversample bits. The actual number
+   * of oversampled values is 2^bits. The oversampling is done automatically in the FPGA.
+   *
+   * @param bits The number of oversample bits.
+   */
+  public void setOversampleBits(final int bits) {
+    AnalogJNI.setAnalogOversampleBits(m_port, bits);
+  }
+
+  /**
+   * Get the number of oversample bits. This gets the number of oversample bits from the FPGA. The
+   * actual number of oversampled values is 2^bits. The oversampling is done automatically in the
+   * FPGA.
+   *
+   * @return The number of oversample bits.
+   */
+  public int getOversampleBits() {
+    return AnalogJNI.getAnalogOversampleBits(m_port);
+  }
+
+  /**
+   * Set the sample rate per channel.
+   *
+   * <p>This is a global setting for all channels. The maximum rate is 500kS/s divided by the number
+   * of channels in use. This is 62500 samples/s per channel if all 8 channels are used.
+   *
+   * @param samplesPerSecond The number of samples per second.
+   */
+  public static void setGlobalSampleRate(final double samplesPerSecond) {
+    AnalogJNI.setAnalogSampleRate(samplesPerSecond);
+  }
+
+  /**
+   * Get the current sample rate.
+   *
+   * <p>This assumes one entry in the scan list. This is a global setting for all channels.
+   *
+   * @return Sample rate.
+   */
+  public static double getGlobalSampleRate() {
+    return AnalogJNI.getAnalogSampleRate();
+  }
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  public void setSimDevice(SimDevice device) {
+    AnalogJNI.setAnalogInputSimDevice(m_port, device.getNativeHandle());
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Analog Input");
+    builder.addDoubleProperty("Value", this::getAverageVoltage, null);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/discrete/CounterBase.java

@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * Interface for counting the number of ticks on a digital input channel. Encoders, Gear tooth
+ * sensors, and counters should all subclass this in order to build more advanced classes for
+ * control and driving.
+ *
+ * <p>All counters will immediately start counting - reset() them if you need them to be zeroed
+ * before use.
+ */
+public interface CounterBase {
+  /** The number of edges for the CounterBase to increment or decrement on. */
+  enum EncodingType {
+    /** Count only the rising edge. */
+    k1X(0),
+    /** Count both the rising and falling edge. */
+    k2X(1),
+    /** Count rising and falling on both channels. */
+    k4X(2);
+
+    /** EncodingType value. */
+    public final int value;
+
+    EncodingType(int value) {
+      this.value = value;
+    }
+  }
+
+  /**
+   * Get the count.
+   *
+   * @return the count
+   */
+  int get();
+
+  /** Reset the count to zero. */
+  void reset();
+
+  /**
+   * Get the time between the last two edges counted.
+   *
+   * @return the time between the last two ticks in seconds
+   */
+  double getPeriod();
+
+  /**
+   * Set the maximum time between edges to be considered stalled.
+   *
+   * @param maxPeriod the maximum period in seconds
+   */
+  void setMaxPeriod(double maxPeriod);
+
+  /**
+   * Determine if the counter is not moving.
+   *
+   * @return true if the counter has not changed for the max period
+   */
+  boolean getStopped();
+
+  /**
+   * Determine which direction the counter is going.
+   *
+   * @return true for one direction, false for the other
+   */
+  boolean getDirection();
+}

wpilibj/src/main/java/org/wpilib/hardware/discrete/DigitalInput.java

@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.DIOJNI;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Class to read a digital input. This class will read digital inputs and return the current value
+ * on the channel. Other devices such as encoders, gear tooth sensors, etc. that are implemented
+ * elsewhere will automatically allocate digital inputs and outputs as required. This class is only
+ * for devices like switches etc. that aren't implemented anywhere else.
+ */
+public class DigitalInput implements AutoCloseable, Sendable {
+  private final int m_channel;
+  private int m_handle;
+
+  /**
+   * Create an instance of a Digital Input class. Creates a digital input given a channel.
+   *
+   * @param channel the DIO channel for the digital input 0-9 are on-board, 10-25 are on the MXP
+   */
+  @SuppressWarnings("this-escape")
+  public DigitalInput(int channel) {
+    SensorUtil.checkDigitalChannel(channel);
+    m_channel = channel;
+
+    m_handle = DIOJNI.initializeDIOPort(channel, true);
+
+    HAL.reportUsage("IO", channel, "DigitalInput");
+    SendableRegistry.add(this, "DigitalInput", channel);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    DIOJNI.freeDIOPort(m_handle);
+    m_handle = 0;
+  }
+
+  /**
+   * Get the value from a digital input channel. Retrieve the value of a single digital input
+   * channel from the FPGA.
+   *
+   * @return the status of the digital input
+   */
+  public boolean get() {
+    return DIOJNI.getDIO(m_handle);
+  }
+
+  /**
+   * Get the channel of the digital input.
+   *
+   * @return The GPIO channel number that this object represents.
+   */
+  public int getChannel() {
+    return m_channel;
+  }
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  public void setSimDevice(SimDevice device) {
+    DIOJNI.setDIOSimDevice(m_handle, device.getNativeHandle());
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Digital Input");
+    builder.addBooleanProperty("Value", this::get, null);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/discrete/DigitalOutput.java

@@ -0,0 +1,199 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.DIOJNI;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Class to write digital outputs. This class will write digital outputs. Other devices that are
+ * implemented elsewhere will automatically allocate digital inputs and outputs as required.
+ */
+public class DigitalOutput implements AutoCloseable, Sendable {
+  private static final int invalidPwmGenerator = 0;
+  private int m_pwmGenerator = invalidPwmGenerator;
+
+  private final int m_channel;
+  private int m_handle;
+
+  /**
+   * Create an instance of a digital output. Create an instance of a digital output given a channel.
+   *
+   * @param channel the DIO channel to use for the digital output. 0-9 are on-board, 10-25 are on
+   *     the MXP
+   */
+  @SuppressWarnings("this-escape")
+  public DigitalOutput(int channel) {
+    SensorUtil.checkDigitalChannel(channel);
+    m_channel = channel;
+
+    m_handle = DIOJNI.initializeDIOPort(channel, false);
+
+    HAL.reportUsage("IO", channel, "DigitalOutput");
+    SendableRegistry.add(this, "DigitalOutput", channel);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    // Disable the pwm only if we have allocated it
+    if (m_pwmGenerator != invalidPwmGenerator) {
+      disablePWM();
+    }
+    DIOJNI.freeDIOPort(m_handle);
+    m_handle = 0;
+  }
+
+  /**
+   * Set the value of a digital output.
+   *
+   * @param value true is on, off is false
+   */
+  public void set(boolean value) {
+    DIOJNI.setDIO(m_handle, value);
+  }
+
+  /**
+   * Gets the value being output from the Digital Output.
+   *
+   * @return the state of the digital output.
+   */
+  public boolean get() {
+    return DIOJNI.getDIO(m_handle);
+  }
+
+  /**
+   * Get the GPIO channel number that this object represents.
+   *
+   * @return The GPIO channel number.
+   */
+  public int getChannel() {
+    return m_channel;
+  }
+
+  /**
+   * Output a single pulse on the digital output line.
+   *
+   * <p>Send a single pulse on the digital output line where the pulse duration is specified in
+   * seconds. Maximum of 65535 microseconds.
+   *
+   * @param pulseLength The pulse length in seconds
+   */
+  public void pulse(final double pulseLength) {
+    DIOJNI.pulse(m_handle, pulseLength);
+  }
+
+  /**
+   * Determine if the pulse is still going. Determine if a previously started pulse is still going.
+   *
+   * @return true if pulsing
+   */
+  public boolean isPulsing() {
+    return DIOJNI.isPulsing(m_handle);
+  }
+
+  /**
+   * Change the PWM frequency of the PWM output on a Digital Output line.
+   *
+   * <p>The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is logarithmic.
+   *
+   * <p>There is only one PWM frequency for all channels.
+   *
+   * @param rate The frequency to output all digital output PWM signals.
+   */
+  public void setPWMRate(double rate) {
+    DIOJNI.setDigitalPWMRate(rate);
+  }
+
+  /**
+   * Enable a PWM PPS (Pulse Per Second) Output on this line.
+   *
+   * <p>Allocate one of the 6 DO PWM generator resources.
+   *
+   * <p>Supply the duty-cycle to output.
+   *
+   * <p>The resolution of the duty cycle is 8-bit.
+   *
+   * @param dutyCycle The duty-cycle to start generating. [0..1]
+   */
+  public void enablePPS(double dutyCycle) {
+    if (m_pwmGenerator != invalidPwmGenerator) {
+      return;
+    }
+    m_pwmGenerator = DIOJNI.allocateDigitalPWM();
+    DIOJNI.setDigitalPWMPPS(m_pwmGenerator, dutyCycle);
+    DIOJNI.setDigitalPWMOutputChannel(m_pwmGenerator, m_channel);
+  }
+
+  /**
+   * Enable a PWM Output on this line.
+   *
+   * <p>Allocate one of the 6 DO PWM generator resources.
+   *
+   * <p>Supply the initial duty-cycle to output in order to avoid a glitch when first starting.
+   *
+   * <p>The resolution of the duty cycle is 8-bit for low frequencies (1kHz or less) but is reduced
+   * the higher the frequency of the PWM signal is.
+   *
+   * @param initialDutyCycle The duty-cycle to start generating. [0..1]
+   */
+  public void enablePWM(double initialDutyCycle) {
+    if (m_pwmGenerator != invalidPwmGenerator) {
+      return;
+    }
+    m_pwmGenerator = DIOJNI.allocateDigitalPWM();
+    DIOJNI.setDigitalPWMDutyCycle(m_pwmGenerator, initialDutyCycle);
+    DIOJNI.setDigitalPWMOutputChannel(m_pwmGenerator, m_channel);
+  }
+
+  /**
+   * Change this line from a PWM output back to a static Digital Output line.
+   *
+   * <p>Free up one of the 6 DO PWM generator resources that were in use.
+   */
+  public void disablePWM() {
+    if (m_pwmGenerator == invalidPwmGenerator) {
+      return;
+    }
+    // Disable the output by routing to a dead bit.
+    DIOJNI.setDigitalPWMOutputChannel(m_pwmGenerator, SensorUtil.kDigitalChannels);
+    DIOJNI.freeDigitalPWM(m_pwmGenerator);
+    m_pwmGenerator = invalidPwmGenerator;
+  }
+
+  /**
+   * Change the duty-cycle that is being generated on the line.
+   *
+   * <p>The resolution of the duty cycle is 8-bit for low frequencies (1kHz or less) but is reduced
+   * the higher the frequency of the PWM signal is.
+   *
+   * @param dutyCycle The duty-cycle to change to. [0..1]
+   */
+  public void updateDutyCycle(double dutyCycle) {
+    if (m_pwmGenerator == invalidPwmGenerator) {
+      return;
+    }
+    DIOJNI.setDigitalPWMDutyCycle(m_pwmGenerator, dutyCycle);
+  }
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  public void setSimDevice(SimDevice device) {
+    DIOJNI.setDIOSimDevice(m_handle, device.getNativeHandle());
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Digital Output");
+    builder.addBooleanProperty("Value", this::get, this::set);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/discrete/PWM.java

@@ -0,0 +1,160 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.PWMJNI;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Class implements the PWM generation in the FPGA.
+ *
+ * <p>The values supplied as arguments for PWM outputs range from -1.0 to 1.0. They are mapped to
+ * the microseconds to keep the pulse high, with a range of 0 (off) to 4096. Changes are immediately
+ * sent to the FPGA, and the update occurs at the next FPGA cycle (5.05ms). There is no delay.
+ */
+public class PWM implements Sendable, AutoCloseable {
+  /** Represents the output period in microseconds. */
+  public enum OutputPeriod {
+    /** Pulse every 5ms. */
+    k5Ms,
+    /** Pulse every 10ms. */
+    k10Ms,
+    /** Pulse every 20ms. */
+    k20Ms
+  }
+
+  private final int m_channel;
+
+  private int m_handle;
+
+  /**
+   * Allocate a PWM given a channel.
+   *
+   * <p>Checks channel value range and allocates the appropriate channel. The allocation is only
+   * done to help users ensure that they don't double assign channels.
+   *
+   * <p>By default, adds itself to SendableRegistry.
+   *
+   * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
+   */
+  public PWM(final int channel) {
+    this(channel, true);
+  }
+
+  /**
+   * Allocate a PWM given a channel.
+   *
+   * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
+   * @param registerSendable If true, adds this instance to SendableRegistry
+   */
+  @SuppressWarnings("this-escape")
+  public PWM(final int channel, final boolean registerSendable) {
+    SensorUtil.checkPWMChannel(channel);
+    m_channel = channel;
+
+    m_handle = PWMJNI.initializePWMPort(channel);
+
+    setDisabled();
+
+    HAL.reportUsage("IO", channel, "PWM");
+    if (registerSendable) {
+      SendableRegistry.add(this, "PWM", channel);
+    }
+  }
+
+  /** Free the resource associated with the PWM channel and set the value to 0. */
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    if (m_handle == 0) {
+      return;
+    }
+    setDisabled();
+    PWMJNI.freePWMPort(m_handle);
+    m_handle = 0;
+  }
+
+  /**
+   * Gets the channel number associated with the PWM Object.
+   *
+   * @return The channel number.
+   */
+  public int getChannel() {
+    return m_channel;
+  }
+
+  /**
+   * Set the PWM value directly to the hardware.
+   *
+   * <p>Write a microsecond pulse value to a PWM channel.
+   *
+   * @param microsecondPulseTime Microsecond pulse PWM value. Range 0 - 4096.
+   */
+  public void setPulseTimeMicroseconds(int microsecondPulseTime) {
+    PWMJNI.setPulseTimeMicroseconds(m_handle, microsecondPulseTime);
+  }
+
+  /**
+   * Get the PWM value directly from the hardware.
+   *
+   * <p>Read a raw value from a PWM channel.
+   *
+   * @return Microsecond pulse PWM control value. Range: 0 - 4096.
+   */
+  public int getPulseTimeMicroseconds() {
+    return PWMJNI.getPulseTimeMicroseconds(m_handle);
+  }
+
+  /** Temporarily disables the PWM output. The next set call will re-enable the output. */
+  public final void setDisabled() {
+    setPulseTimeMicroseconds(0);
+  }
+
+  /**
+   * Sets the PWM output period.
+   *
+   * @param mult The output period to apply to this channel
+   */
+  public void setOutputPeriod(OutputPeriod mult) {
+    int scale =
+        switch (mult) {
+          case k20Ms -> 3;
+          case k10Ms -> 1;
+          case k5Ms -> 0;
+        };
+
+    PWMJNI.setPWMOutputPeriod(m_handle, scale);
+  }
+
+  /**
+   * Get the underlying handle.
+   *
+   * @return Underlying PWM handle
+   */
+  public int getHandle() {
+    return m_handle;
+  }
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  public void setSimDevice(SimDevice device) {
+    PWMJNI.setPWMSimDevice(m_handle, device.getNativeHandle());
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("PWM");
+    builder.setActuator(true);
+    builder.addDoubleProperty(
+        "Value", this::getPulseTimeMicroseconds, value -> setPulseTimeMicroseconds((int) value));
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/imu/OnboardIMU.java

@@ -0,0 +1,196 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.IMUJNI;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+
+/** SystemCore onboard IMU. */
+public class OnboardIMU {
+  /** A mount orientation of SystemCore. */
+  public enum MountOrientation {
+    /** Flat (mounted parallel to the ground). */
+    kFlat,
+    /** Landscape (vertically mounted with long edge of SystemCore parallel to the ground). */
+    kLandscape,
+    /** Portrait (vertically mounted with the short edge of SystemCore parallel to the ground). */
+    kPortrait
+  }
+
+  /**
+   * Constructs a handle to the SystemCore onboard IMU.
+   *
+   * @param mountOrientation the mount orientation of SystemCore to determine yaw.
+   */
+  public OnboardIMU(MountOrientation mountOrientation) {
+    m_mountOrientation = mountOrientation;
+  }
+
+  /**
+   * Get the yaw value in radians.
+   *
+   * @return yaw value in radians
+   */
+  public double getYawRadians() {
+    return getYawNoOffset() - m_yawOffset;
+  }
+
+  /**
+   * Reset the current yaw value to 0. Future reads of the yaw value will be relative to the current
+   * orientation.
+   */
+  public void resetYaw() {
+    m_yawOffset = getYawNoOffset();
+  }
+
+  /**
+   * Get the yaw as a Rotation2d.
+   *
+   * @return yaw
+   */
+  public Rotation2d getRotation2d() {
+    return new Rotation2d(getYawRadians());
+  }
+
+  /**
+   * Get the 3D orientation as a Rotation3d.
+   *
+   * @return 3D orientation
+   */
+  public Rotation3d getRotation3d() {
+    return new Rotation3d(getQuaternion());
+  }
+
+  /**
+   * Get the 3D orientation as a Quaternion.
+   *
+   * @return 3D orientation
+   */
+  public Quaternion getQuaternion() {
+    double[] quatRaw = new double[4];
+    IMUJNI.getIMUQuaternion(quatRaw);
+    return new Quaternion(quatRaw[0], quatRaw[1], quatRaw[2], quatRaw[3]);
+  }
+
+  /**
+   * Get the angle about the X axis of the IMU in radians.
+   *
+   * @return angle about the X axis in radians
+   */
+  public double getAngleX() {
+    return getRawEulerAngles()[0];
+  }
+
+  /**
+   * Get the angle about the Y axis of the IMU in radians.
+   *
+   * @return angle about the Y axis in radians
+   */
+  public double getAngleY() {
+    return getRawEulerAngles()[1];
+  }
+
+  /**
+   * Get the angle about the Z axis of the IMU in radians.
+   *
+   * @return angle about the Z axis in radians
+   */
+  public double getAngleZ() {
+    return getRawEulerAngles()[2];
+  }
+
+  /**
+   * Get the angular rate about the X axis of the IMU in radians per second.
+   *
+   * @return angular rate about the X axis in radians per second
+   */
+  public double getGyroRateX() {
+    return getRawGyroRates()[0];
+  }
+
+  /**
+   * Get the angular rate about the Y axis of the IMU in radians per second.
+   *
+   * @return angular rate about the Y axis in radians per second
+   */
+  public double getGyroRateY() {
+    return getRawGyroRates()[1];
+  }
+
+  /**
+   * Get the angular rate about the Z axis of the IMU in radians per second.
+   *
+   * @return angular rate about the Z axis in radians per second
+   */
+  public double getGyroRateZ() {
+    return getRawGyroRates()[2];
+  }
+
+  /**
+   * Get the acceleration along the X axis of the IMU in meters per second squared.
+   *
+   * @return acceleration along the X axis in meters per second squared
+   */
+  public double getAccelX() {
+    return getRawAccels()[0];
+  }
+
+  /**
+   * Get the acceleration along the X axis of the IMU in meters per second squared.
+   *
+   * @return acceleration along the X axis in meters per second squared
+   */
+  public double getAccelY() {
+    return getRawAccels()[1];
+  }
+
+  /**
+   * Get the acceleration along the X axis of the IMU in meters per second squared.
+   *
+   * @return acceleration along the X axis in meters per second squared
+   */
+  public double getAccelZ() {
+    return getRawAccels()[2];
+  }
+
+  private double[] getRawEulerAngles() {
+    double[] anglesRaw = new double[3];
+    switch (m_mountOrientation) {
+      case kFlat -> IMUJNI.getIMUEulerAnglesFlat(anglesRaw);
+      case kLandscape -> IMUJNI.getIMUEulerAnglesLandscape(anglesRaw);
+      case kPortrait -> IMUJNI.getIMUEulerAnglesPortrait(anglesRaw);
+      default -> {
+        // NOP
+      }
+    }
+    return anglesRaw;
+  }
+
+  private double[] getRawGyroRates() {
+    double[] ratesRaw = new double[3];
+    IMUJNI.getIMUGyroRates(ratesRaw);
+    return ratesRaw;
+  }
+
+  private double[] getRawAccels() {
+    double[] accelsRaw = new double[3];
+    IMUJNI.getIMUAcceleration(accelsRaw);
+    return accelsRaw;
+  }
+
+  private double getYawNoOffset() {
+    return switch (m_mountOrientation) {
+      case kFlat -> IMUJNI.getIMUYawFlat();
+      case kLandscape -> IMUJNI.getIMUYawLandscape();
+      case kPortrait -> IMUJNI.getIMUYawPortrait();
+      default -> 0;
+    };
+  }
+
+  private final MountOrientation m_mountOrientation;
+  private double m_yawOffset;
+}

wpilibj/src/main/java/org/wpilib/hardware/led/AddressableLED.java

@@ -0,0 +1,164 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AddressableLEDJNI;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * A class for driving addressable LEDs, such as WS2812B, WS2815, and NeoPixels.
+ *
+ * <p>Some LEDs use a different color order than the default GRB. The color order is configurable
+ * using {@link #setColorOrder(ColorOrder)}.
+ *
+ * <p>Up to 1024 LEDs may be controlled in total across all AddressableLED instances. A single
+ * global buffer is used for all instances. The start position used for LED data for the output is
+ * set via SetStart() and the length of the strip is set via SetLength(). Both of these default to
+ * zero, so multiple instances will access the same pixel data unless SetStart() is called to adjust
+ * the starting point.
+ */
+public class AddressableLED implements AutoCloseable {
+  /** Order that color data is sent over the wire. */
+  public enum ColorOrder {
+    /** RGB order. */
+    kRGB(AddressableLEDJNI.COLOR_ORDER_RGB),
+    /** RBG order. */
+    kRBG(AddressableLEDJNI.COLOR_ORDER_RBG),
+    /** BGR order. */
+    kBGR(AddressableLEDJNI.COLOR_ORDER_BGR),
+    /** BRG order. */
+    kBRG(AddressableLEDJNI.COLOR_ORDER_BRG),
+    /** GBR order. */
+    kGBR(AddressableLEDJNI.COLOR_ORDER_GBR),
+    /** GRB order. This is the default order. */
+    kGRB(AddressableLEDJNI.COLOR_ORDER_GRB);
+
+    /** The native value for this ColorOrder. */
+    public final int value;
+
+    ColorOrder(int value) {
+      this.value = value;
+    }
+
+    /**
+     * Gets a color order from an int value.
+     *
+     * @param value int value
+     * @return color order
+     */
+    public ColorOrder fromValue(int value) {
+      return switch (value) {
+        case AddressableLEDJNI.COLOR_ORDER_RBG -> kRBG;
+        case AddressableLEDJNI.COLOR_ORDER_BGR -> kBGR;
+        case AddressableLEDJNI.COLOR_ORDER_BRG -> kBRG;
+        case AddressableLEDJNI.COLOR_ORDER_GRB -> kGRB;
+        case AddressableLEDJNI.COLOR_ORDER_GBR -> kGBR;
+        case AddressableLEDJNI.COLOR_ORDER_RGB -> kRGB;
+        default -> kGRB;
+      };
+    }
+  }
+
+  private final int m_channel;
+  private final int m_handle;
+  private int m_start;
+  private int m_length;
+  private ColorOrder m_colorOrder = ColorOrder.kGRB;
+
+  /**
+   * Constructs a new driver for a specific channel.
+   *
+   * @param channel the output channel to use
+   */
+  public AddressableLED(int channel) {
+    m_channel = channel;
+    m_handle = AddressableLEDJNI.initialize(channel);
+    HAL.reportUsage("IO", channel, "AddressableLED");
+  }
+
+  @Override
+  public void close() {
+    if (m_handle != 0) {
+      AddressableLEDJNI.free(m_handle);
+    }
+  }
+
+  /**
+   * Gets the output channel.
+   *
+   * @return the output channel
+   */
+  public int getChannel() {
+    return m_channel;
+  }
+
+  /**
+   * Sets the color order for this AddressableLED. The default order is GRB.
+   *
+   * <p>This will take effect on the next call to {@link #setData(AddressableLEDBuffer)}.
+   *
+   * @param order the color order
+   */
+  public void setColorOrder(ColorOrder order) {
+    m_colorOrder = order;
+  }
+
+  /**
+   * Sets the display start of the LED strip in the global buffer.
+   *
+   * @param start the strip start, in LEDs
+   */
+  public void setStart(int start) {
+    m_start = start;
+    AddressableLEDJNI.setStart(m_handle, start);
+  }
+
+  /**
+   * Gets the display start of the LED strip in the global buffer.
+   *
+   * @return the strip start, in LEDs
+   */
+  public int getStart() {
+    return m_start;
+  }
+
+  /**
+   * Sets the length of the LED strip.
+   *
+   * @param length the strip length, in LEDs
+   */
+  public void setLength(int length) {
+    m_length = length;
+    AddressableLEDJNI.setLength(m_handle, length);
+  }
+
+  /**
+   * Sets the LED output data.
+   *
+   * <p>This will write to the global buffer starting at the location set by setStart() and up to
+   * the length set by setLength().
+   *
+   * @param buffer the buffer to write
+   */
+  public void setData(AddressableLEDBuffer buffer) {
+    AddressableLEDJNI.setData(
+        m_start,
+        m_colorOrder.value,
+        buffer.m_buffer,
+        0,
+        3 * Math.min(m_length, buffer.getLength()));
+  }
+
+  /**
+   * Sets the LED output data at an arbitrary location in the global buffer.
+   *
+   * @param start the start location, in LEDs
+   * @param colorOrder the color order
+   * @param buffer the buffer to write
+   */
+  public static void setGlobalData(int start, ColorOrder colorOrder, AddressableLEDBuffer buffer) {
+    AddressableLEDJNI.setData(start, colorOrder.value, buffer.m_buffer);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/led/AddressableLEDBuffer.java

@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+/** Buffer storage for Addressable LEDs. */
+public class AddressableLEDBuffer implements LEDReader, LEDWriter {
+  byte[] m_buffer;
+
+  /**
+   * Constructs a new LED buffer with the specified length.
+   *
+   * @param length The length of the buffer in pixels
+   */
+  public AddressableLEDBuffer(int length) {
+    m_buffer = new byte[length * 3];
+  }
+
+  /**
+   * Sets a specific led in the buffer.
+   *
+   * @param index the index to write
+   * @param r the r value [0-255]
+   * @param g the g value [0-255]
+   * @param b the b value [0-255]
+   */
+  @Override
+  public void setRGB(int index, int r, int g, int b) {
+    m_buffer[index * 3] = (byte) r;
+    m_buffer[(index * 3) + 1] = (byte) g;
+    m_buffer[(index * 3) + 2] = (byte) b;
+  }
+
+  /**
+   * Gets the buffer length.
+   *
+   * @return the buffer length
+   */
+  @Override
+  public int getLength() {
+    return m_buffer.length / 3;
+  }
+
+  /**
+   * Gets the red channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the red channel, from [0, 255]
+   */
+  @Override
+  public int getRed(int index) {
+    return m_buffer[index * 3] & 0xFF;
+  }
+
+  /**
+   * Gets the green channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the green channel, from [0, 255]
+   */
+  @Override
+  public int getGreen(int index) {
+    return m_buffer[index * 3 + 1] & 0xFF;
+  }
+
+  /**
+   * Gets the blue channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the blue channel, from [0, 255]
+   */
+  @Override
+  public int getBlue(int index) {
+    return m_buffer[index * 3 + 2] & 0xFF;
+  }
+
+  /**
+   * Creates a view of a subsection of this data buffer, starting from (and including) {@code
+   * startingIndex} and ending on (and including) {@code endingIndex}. Views cannot be written
+   * directly to an {@link AddressableLED}, but are useful tools for logically separating different
+   * sections of an LED strip for independent control.
+   *
+   * @param startingIndex the first index in this buffer that the view should encompass (inclusive)
+   * @param endingIndex the last index in this buffer that the view should encompass (inclusive)
+   * @return the view object
+   */
+  public AddressableLEDBufferView createView(int startingIndex, int endingIndex) {
+    return new AddressableLEDBufferView(this, startingIndex, endingIndex);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/led/AddressableLEDBufferView.java

@@ -0,0 +1,163 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+/**
+ * A view of another addressable LED buffer. Views CANNOT be written directly to an LED strip; the
+ * backing buffer must be written instead. However, views provide an easy way to split a large LED
+ * strip into smaller sections (which may be reversed from the orientation of the LED strip as a
+ * whole) that can be animated individually without modifying LEDs outside those sections.
+ */
+public class AddressableLEDBufferView implements LEDReader, LEDWriter {
+  private final LEDReader m_backingReader;
+  private final LEDWriter m_backingWriter;
+  private final int m_startingIndex;
+  private final int m_endingIndex;
+  private final int m_length;
+
+  /**
+   * Creates a new view of a buffer. A view will be reversed if the starting index is after the
+   * ending index; writing front-to-back in the view will write in the back-to-front direction on
+   * the underlying buffer.
+   *
+   * @param backingBuffer the backing buffer to view
+   * @param startingIndex the index of the LED in the backing buffer that the view should start from
+   * @param endingIndex the index of the LED in the backing buffer that the view should end on
+   * @param <B> the type of the buffer object to create a view for
+   */
+  public <B extends LEDReader & LEDWriter> AddressableLEDBufferView(
+      B backingBuffer, int startingIndex, int endingIndex) {
+    this(
+        requireNonNullParam(backingBuffer, "backingBuffer", "AddressableLEDBufferView"),
+        backingBuffer,
+        startingIndex,
+        endingIndex);
+  }
+
+  /**
+   * Creates a new view of a buffer. A view will be reversed if the starting index is after the
+   * ending index; writing front-to-back in the view will write in the back-to-front direction on
+   * the underlying buffer.
+   *
+   * @param backingReader the backing LED data reader
+   * @param backingWriter the backing LED data writer
+   * @param startingIndex the index of the LED in the backing buffer that the view should start from
+   * @param endingIndex the index of the LED in the backing buffer that the view should end on
+   */
+  public AddressableLEDBufferView(
+      LEDReader backingReader, LEDWriter backingWriter, int startingIndex, int endingIndex) {
+    requireNonNullParam(backingReader, "backingReader", "AddressableLEDBufferView");
+    requireNonNullParam(backingWriter, "backingWriter", "AddressableLEDBufferView");
+
+    if (startingIndex < 0 || startingIndex >= backingReader.getLength()) {
+      throw new IndexOutOfBoundsException("Start index out of range: " + startingIndex);
+    }
+    if (endingIndex < 0 || endingIndex >= backingReader.getLength()) {
+      throw new IndexOutOfBoundsException("End index out of range: " + endingIndex);
+    }
+
+    m_backingReader = backingReader;
+    m_backingWriter = backingWriter;
+
+    m_startingIndex = startingIndex;
+    m_endingIndex = endingIndex;
+    m_length = Math.abs(endingIndex - startingIndex) + 1;
+  }
+
+  /**
+   * Creates a view that operates on the same range as this one, but goes in reverse order. This is
+   * useful for serpentine runs of LED strips connected front-to-end; simply reverse the view for
+   * reversed sections and animations will move in the same physical direction along both strips.
+   *
+   * @return the reversed view
+   */
+  public AddressableLEDBufferView reversed() {
+    return new AddressableLEDBufferView(this, m_length - 1, 0);
+  }
+
+  @Override
+  public int getLength() {
+    return m_length;
+  }
+
+  @Override
+  public void setRGB(int index, int r, int g, int b) {
+    m_backingWriter.setRGB(nativeIndex(index), r, g, b);
+  }
+
+  @Override
+  public Color getLED(int index) {
+    // override to delegate to the backing buffer to avoid 3x native index lookups & bounds checks
+    return m_backingReader.getLED(nativeIndex(index));
+  }
+
+  @Override
+  public Color8Bit getLED8Bit(int index) {
+    // override to delegate to the backing buffer to avoid 3x native index lookups & bounds checks
+    return m_backingReader.getLED8Bit(nativeIndex(index));
+  }
+
+  @Override
+  public int getRed(int index) {
+    return m_backingReader.getRed(nativeIndex(index));
+  }
+
+  @Override
+  public int getGreen(int index) {
+    return m_backingReader.getGreen(nativeIndex(index));
+  }
+
+  @Override
+  public int getBlue(int index) {
+    return m_backingReader.getBlue(nativeIndex(index));
+  }
+
+  /**
+   * Checks if this view is reversed with respect to its backing buffer.
+   *
+   * @return true if the view is reversed, false otherwise
+   */
+  public boolean isReversed() {
+    return m_endingIndex < m_startingIndex;
+  }
+
+  /**
+   * Converts a view-local index in the range [start, end] to a global index in the range [0,
+   * length].
+   *
+   * @param viewIndex the view-local index
+   * @return the corresponding global index
+   * @throws IndexOutOfBoundsException if the view index is not contained within the bounds of this
+   *     view
+   */
+  private int nativeIndex(int viewIndex) {
+    if (isReversed()) {
+      //  0  1  2  3   4  5  6  7   8  9  10
+      //  ↓  ↓  ↓  ↓   ↓  ↓  ↓  ↓   ↓  ↓  ↓
+      // [_, _, _, _, (d, c, b, a), _, _, _]
+      //               ↑  ↑  ↑  ↑
+      //               3  2  1  0
+      if (viewIndex < 0 || viewIndex > m_startingIndex) {
+        throw new IndexOutOfBoundsException(viewIndex);
+      }
+      return m_startingIndex - viewIndex;
+    } else {
+      //  0  1  2  3   4  5  6  7   8  9  10
+      //  ↓  ↓  ↓  ↓   ↓  ↓  ↓  ↓   ↓  ↓  ↓
+      // [_, _, _, _, (a, b, c, d), _, _, _]
+      //               ↑  ↑  ↑  ↑
+      //               0  1  2  3
+      if (viewIndex < 0 || viewIndex > m_endingIndex) {
+        throw new IndexOutOfBoundsException(viewIndex);
+      }
+      return m_startingIndex + viewIndex;
+    }
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/led/LEDPattern.java

@@ -0,0 +1,689 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Microsecond;
+import static edu.wpi.first.units.Units.Microseconds;
+import static edu.wpi.first.units.Units.Value;
+
+import edu.wpi.first.units.collections.LongToObjectHashMap;
+import edu.wpi.first.units.measure.Dimensionless;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Frequency;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.wpilibj.util.Color;
+import java.util.Map;
+import java.util.Objects;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+/**
+ * An LED pattern controls lights on an LED strip to command patterns of color that may change over
+ * time. Dynamic patterns should synchronize on an external clock for timed-based animations ({@link
+ * WPIUtilJNI#now()} is recommended, since it can be mocked in simulation and unit tests), or on
+ * some other dynamic input (see {@link #synchronizedBlink(BooleanSupplier)}, for example).
+ *
+ * <p>Patterns should be updated periodically in order for animations to play smoothly. For example,
+ * a hypothetical LED subsystem could create a {@code Command} that will continuously apply the
+ * pattern to its LED data buffer as part of the main periodic loop.
+ *
+ * <pre><code>
+ *   public class LEDs extends SubsystemBase {
+ *     private final AddressableLED m_led = new AddressableLED(0);
+ *     private final AddressableLEDBuffer m_ledData = new AddressableLEDBuffer(120);
+ *
+ *     public LEDs() {
+ *       m_led.setLength(120);
+ *       m_led.start();
+ *     }
+ *
+ *    {@literal @}Override
+ *     public void periodic() {
+ *       m_led.writeData(m_ledData);
+ *     }
+ *
+ *     public Command runPattern(LEDPattern pattern) {
+ *       return run(() -> pattern.applyTo(m_ledData));
+ *     }
+ *   }
+ * </code></pre>
+ *
+ * <p>LED patterns are stateless, and as such can be applied to multiple LED strips (or different
+ * sections of the same LED strip, since the roboRIO can only drive a single LED strip). In this
+ * example, we split the single buffer into two views - one for the section of the LED strip on the
+ * left side of a robot, and another view for the section of LEDs on the right side. The same
+ * pattern is able to be applied to both sides.
+ *
+ * <pre><code>
+ *   public class LEDs extends SubsystemBase {
+ *     private final AddressableLED m_led = new AddressableLED(0);
+ *     private final AddressableLEDBuffer m_ledData = new AddressableLEDBuffer(60);
+ *     private final AddressableLEDBufferView m_leftData = m_ledData.createView(0, 29);
+ *     private final AddressableLEDBufferView m_rightData = m_ledData.createView(30, 59).reversed();
+ *
+ *     public LEDs() {
+ *       m_led.setLength(60);
+ *       m_led.start();
+ *     }
+ *
+ *    {@literal @}Override
+ *     public void periodic() {
+ *       m_led.writeData(m_ledData);
+ *     }
+ *
+ *     public Command runPattern(LEDPattern pattern) {
+ *       // Use the single input pattern to drive both sides
+ *       return runSplitPatterns(pattern, pattern);
+ *     }
+ *
+ *     public Command runSplitPatterns(LEDPattern left, LEDPattern right) {
+ *       return run(() -> {
+ *         left.applyTo(m_leftData);
+ *         right.applyTo(m_rightData);
+ *       });
+ *     }
+ *   }
+ * </code></pre>
+ */
+@FunctionalInterface
+public interface LEDPattern {
+  /** A functional interface for index mapping functions. */
+  @FunctionalInterface
+  interface IndexMapper {
+    /**
+     * Maps the index.
+     *
+     * @param bufLen Length of the buffer
+     * @param index The index to map
+     * @return The mapped index
+     */
+    int apply(int bufLen, int index);
+  }
+
+  /**
+   * Writes the pattern to an LED buffer. Dynamic animations should be called periodically (such as
+   * with a command or with a periodic method) to refresh the buffer over time.
+   *
+   * <p>This method is intentionally designed to use separate objects for reading and writing data.
+   * By splitting them up, we can easily modify the behavior of some base pattern to make it {@link
+   * #scrollAtRelativeSpeed(Frequency) scroll}, {@link #blink(Time, Time) blink}, or {@link
+   * #breathe(Time) breathe} by intercepting the data writes to transform their behavior to whatever
+   * we like.
+   *
+   * @param reader data reader for accessing buffer length and current colors
+   * @param writer data writer for setting new LED colors on the buffer
+   */
+  void applyTo(LEDReader reader, LEDWriter writer);
+
+  /**
+   * Convenience for {@link #applyTo(LEDReader, LEDWriter)} when one object provides both a read and
+   * a write interface. This is most helpful for playing an animated pattern directly on an {@link
+   * AddressableLEDBuffer} for the sake of code clarity.
+   *
+   * <pre><code>
+   *   AddressableLEDBuffer data = new AddressableLEDBuffer(120);
+   *   LEDPattern pattern = ...
+   *
+   *   void periodic() {
+   *     pattern.applyTo(data);
+   *   }
+   * </code></pre>
+   *
+   * @param readWriter the object to use for both reading and writing to a set of LEDs
+   * @param <T> the type of the object that can both read and write LED data
+   */
+  default <T extends LEDReader & LEDWriter> void applyTo(T readWriter) {
+    applyTo(readWriter, readWriter);
+  }
+
+  /**
+   * Creates a pattern with remapped indices.
+   *
+   * @param indexMapper the index mapper
+   * @return the mapped pattern
+   */
+  default LEDPattern mapIndex(IndexMapper indexMapper) {
+    return (reader, writer) -> {
+      int bufLen = reader.getLength();
+      applyTo(
+          new LEDReader() {
+            @Override
+            public int getLength() {
+              return reader.getLength();
+            }
+
+            @Override
+            public int getRed(int index) {
+              return reader.getRed(indexMapper.apply(bufLen, index));
+            }
+
+            @Override
+            public int getGreen(int index) {
+              return reader.getGreen(indexMapper.apply(bufLen, index));
+            }
+
+            @Override
+            public int getBlue(int index) {
+              return reader.getBlue(indexMapper.apply(bufLen, index));
+            }
+          },
+          (i, r, g, b) -> writer.setRGB(indexMapper.apply(bufLen, i), r, g, b));
+    };
+  }
+
+  /**
+   * Creates a pattern that displays this one in reverse. Scrolling patterns will scroll in the
+   * opposite direction (but at the same speed). It will treat the end of an LED strip as the start,
+   * and the start of the strip as the end. This can be useful for making ping-pong patterns that
+   * travel from one end of an LED strip to the other, then reverse direction and move back to the
+   * start. This can also be useful when working with LED strips connected in a serpentine pattern
+   * (where the start of one strip is connected to the end of the previous one); however, consider
+   * using a {@link AddressableLEDBufferView#reversed() reversed view} of the overall buffer for
+   * that segment rather than reversing patterns.
+   *
+   * @return the reverse pattern
+   * @see AddressableLEDBufferView#reversed()
+   */
+  default LEDPattern reversed() {
+    return mapIndex((length, index) -> length - 1 - index);
+  }
+
+  /**
+   * Creates a pattern that plays this one, but offset by a certain number of LEDs. The offset
+   * pattern will wrap around, if necessary.
+   *
+   * @param offset how many LEDs to offset by
+   * @return the offset pattern
+   */
+  default LEDPattern offsetBy(int offset) {
+    return mapIndex((length, index) -> Math.floorMod(index + offset, length));
+  }
+
+  /**
+   * Creates a pattern that plays this one scrolling up the buffer. The velocity controls how fast
+   * the pattern returns back to its original position, and is in terms of the length of the LED
+   * strip; scrolling across a segment that is 10 LEDs long will travel twice as fast as on a
+   * segment that's only 5 LEDs long (assuming equal LED density on both segments).
+   *
+   * <p>For example, scrolling a pattern by one quarter of any LED strip's length per second,
+   * regardless of the total number of LEDs on that strip:
+   *
+   * <pre>
+   *   LEDPattern rainbow = LEDPattern.rainbow(255, 255);
+   *   LEDPattern scrollingRainbow = rainbow.scrollAtRelativeSpeed(Percent.per(Second).of(25));
+   * </pre>
+   *
+   * @param velocity how fast the pattern should move, in terms of how long it takes to do a full
+   *     scroll along the length of LEDs and return back to the starting position
+   * @return the scrolling pattern
+   */
+  default LEDPattern scrollAtRelativeSpeed(Frequency velocity) {
+    final double periodMicros = velocity.asPeriod().in(Microseconds);
+
+    return mapIndex(
+        (bufLen, index) -> {
+          long now = RobotController.getTime();
+
+          // index should move by (buf.length) / (period)
+          double t = (now % (long) periodMicros) / periodMicros;
+          int offset = (int) (t * bufLen);
+
+          return Math.floorMod(index + offset, bufLen);
+        });
+  }
+
+  /**
+   * Creates a pattern that plays this one scrolling up an LED strip. A negative velocity makes the
+   * pattern play in reverse.
+   *
+   * <p>For example, scrolling a pattern at 4 inches per second along an LED strip with 60 LEDs per
+   * meter:
+   *
+   * <pre>
+   *   // LEDs per meter, a known value taken from the spec sheet of our particular LED strip
+   *   Distance LED_SPACING = Meters.of(1.0 / 60);
+   *
+   *   LEDPattern rainbow = LEDPattern.rainbow();
+   *   LEDPattern scrollingRainbow =
+   *     rainbow.scrollAtAbsoluteSpeed(InchesPerSecond.of(4), LED_SPACING);
+   * </pre>
+   *
+   * <p>Note that this pattern will scroll <i>faster</i> if applied to a less dense LED strip (such
+   * as 30 LEDs per meter), or <i>slower</i> if applied to a denser LED strip (such as 120 or 144
+   * LEDs per meter).
+   *
+   * @param velocity how fast the pattern should move along a physical LED strip
+   * @param ledSpacing the distance between adjacent LEDs on the physical LED strip
+   * @return the scrolling pattern
+   */
+  default LEDPattern scrollAtAbsoluteSpeed(LinearVelocity velocity, Distance ledSpacing) {
+    // eg velocity = 10 m/s, spacing = 0.01m
+    // meters per micro = 1e-5 m/us
+    // micros per LED = 1e-2 m / (1e-5 m/us) = 1e-3 us
+
+    var metersPerMicro = velocity.in(Meters.per(Microsecond));
+    var microsPerLED = (int) (ledSpacing.in(Meters) / metersPerMicro);
+
+    return mapIndex(
+        (bufLen, index) -> {
+          long now = RobotController.getTime();
+
+          // every step in time that's a multiple of microsPerLED will increment the offset by 1
+          var offset = (int) (now / microsPerLED);
+
+          // floorMod so if the offset is negative, we still get positive outputs
+          return Math.floorMod(index + offset, bufLen);
+        });
+  }
+
+  /**
+   * Creates a pattern that switches between playing this pattern and turning the entire LED strip
+   * off.
+   *
+   * @param onTime how long the pattern should play for, per cycle
+   * @param offTime how long the pattern should be turned off for, per cycle
+   * @return the blinking pattern
+   */
+  default LEDPattern blink(Time onTime, Time offTime) {
+    final long totalTimeMicros = (long) (onTime.in(Microseconds) + offTime.in(Microseconds));
+    final long onTimeMicros = (long) onTime.in(Microseconds);
+
+    return (reader, writer) -> {
+      if (RobotController.getTime() % totalTimeMicros < onTimeMicros) {
+        applyTo(reader, writer);
+      } else {
+        kOff.applyTo(reader, writer);
+      }
+    };
+  }
+
+  /**
+   * Like {@link #blink(Time, Time) blink(onTime, offTime)}, but where the "off" time is exactly
+   * equal to the "on" time.
+   *
+   * @param onTime how long the pattern should play for (and be turned off for), per cycle
+   * @return the blinking pattern
+   */
+  default LEDPattern blink(Time onTime) {
+    return blink(onTime, onTime);
+  }
+
+  /**
+   * Creates a pattern that blinks this one on and off in sync with a true/false signal. The pattern
+   * will play while the signal outputs {@code true}, and will turn off while the signal outputs
+   * {@code false}.
+   *
+   * @param signal the signal to synchronize with
+   * @return the blinking pattern
+   */
+  default LEDPattern synchronizedBlink(BooleanSupplier signal) {
+    return (reader, writer) -> {
+      if (signal.getAsBoolean()) {
+        applyTo(reader, writer);
+      } else {
+        kOff.applyTo(reader, writer);
+      }
+    };
+  }
+
+  /**
+   * Creates a pattern that brightens and dims this one over time. Brightness follows a sinusoidal
+   * pattern.
+   *
+   * @param period how fast the breathing pattern should complete a single cycle
+   * @return the breathing pattern
+   */
+  default LEDPattern breathe(Time period) {
+    final long periodMicros = (long) period.in(Microseconds);
+
+    return (reader, writer) -> {
+      applyTo(
+          reader,
+          (i, r, g, b) -> {
+            // How far we are in the cycle, in the range [0, 1)
+            double t = (RobotController.getTime() % periodMicros) / (double) periodMicros;
+            double phase = t * 2 * Math.PI;
+
+            // Apply the cosine function and shift its output from [-1, 1] to [0, 1]
+            // Use cosine so the period starts at 100% brightness
+            double dim = (Math.cos(phase) + 1) / 2.0;
+
+            int output = Color.lerpRGB(0, 0, 0, r, g, b, dim);
+
+            writer.setRGB(
+                i,
+                Color.unpackRGB(output, Color.RGBChannel.kRed),
+                Color.unpackRGB(output, Color.RGBChannel.kGreen),
+                Color.unpackRGB(output, Color.RGBChannel.kBlue));
+          });
+    };
+  }
+
+  /**
+   * Creates a pattern that plays this pattern overlaid on another. Anywhere this pattern sets an
+   * LED to off (or {@link Color#kBlack}), the base pattern will be displayed instead.
+   *
+   * @param base the base pattern to overlay on top of
+   * @return the combined overlay pattern
+   */
+  default LEDPattern overlayOn(LEDPattern base) {
+    return (reader, writer) -> {
+      // write the base pattern down first...
+      base.applyTo(reader, writer);
+
+      // ... then, overwrite with the illuminated LEDs from the overlay
+      applyTo(
+          reader,
+          (i, r, g, b) -> {
+            if (r != 0 || g != 0 || b != 0) {
+              writer.setRGB(i, r, g, b);
+            }
+          });
+    };
+  }
+
+  /**
+   * Creates a pattern that displays outputs as a combination of this pattern and another. Color
+   * values are calculated as the average color of both patterns; if both patterns set the same LED
+   * to the same color, then it is set to that color, but if one pattern sets to one color and the
+   * other pattern sets it to off, then it will show the color of the first pattern but at
+   * approximately half brightness. This is different from {@link #overlayOn}, which will show the
+   * base pattern at full brightness if the overlay is set to off at that position.
+   *
+   * @param other the pattern to blend with
+   * @return the blended pattern
+   */
+  default LEDPattern blend(LEDPattern other) {
+    return (reader, writer) -> {
+      applyTo(reader, writer);
+
+      other.applyTo(
+          reader,
+          (i, r, g, b) -> {
+            int blendedRGB =
+                Color.lerpRGB(
+                    reader.getRed(i), reader.getGreen(i), reader.getBlue(i), r, g, b, 0.5);
+
+            writer.setRGB(
+                i,
+                Color.unpackRGB(blendedRGB, Color.RGBChannel.kRed),
+                Color.unpackRGB(blendedRGB, Color.RGBChannel.kGreen),
+                Color.unpackRGB(blendedRGB, Color.RGBChannel.kBlue));
+          });
+    };
+  }
+
+  /**
+   * Similar to {@link #blend(LEDPattern)}, but performs a bitwise mask on each color channel rather
+   * than averaging the colors for each LED. This can be helpful for displaying only a portion of
+   * the base pattern by applying a mask that sets the desired area to white, and all other areas to
+   * black. However, it can also be used to display only certain color channels or hues; for
+   * example, masking with {@code LEDPattern.color(Color.kRed)} will turn off the green and blue
+   * channels on the output pattern, leaving only the red LEDs to be illuminated.
+   *
+   * @param mask the mask to apply
+   * @return the masked pattern
+   */
+  default LEDPattern mask(LEDPattern mask) {
+    return (reader, writer) -> {
+      // Apply the current pattern down as normal...
+      applyTo(reader, writer);
+
+      mask.applyTo(
+          reader,
+          (i, r, g, b) -> {
+            // ... then perform a bitwise AND operation on each channel to apply the mask
+            writer.setRGB(i, r & reader.getRed(i), g & reader.getGreen(i), b & reader.getBlue(i));
+          });
+    };
+  }
+
+  /**
+   * Creates a pattern that plays this one, but at a different brightness. Brightness multipliers
+   * are applied per-channel in the RGB space; no HSL or HSV conversions are applied. Multipliers
+   * are also uncapped, which may result in the original colors washing out and appearing less
+   * saturated or even just a bright white.
+   *
+   * <p>This method is predominantly intended for dimming LEDs to avoid painfully bright or
+   * distracting patterns from playing (apologies to the 2024 NE Greater Boston field staff).
+   *
+   * <p>For example, dimming can be done simply by adding a call to `atBrightness` at the end of a
+   * pattern:
+   *
+   * <pre>
+   *   // Solid red, but at 50% brightness
+   *   LEDPattern.solid(Color.kRed).atBrightness(Percent.of(50));
+   *
+   *   // Solid white, but at only 10% (i.e. ~0.5V)
+   *   LEDPattern.solid(Color.kWhite).atBrightness(Percent.of(10));
+   * </pre>
+   *
+   * @param relativeBrightness the multiplier to apply to all channels to modify brightness
+   * @return the input pattern, displayed at
+   */
+  default LEDPattern atBrightness(Dimensionless relativeBrightness) {
+    double multiplier = relativeBrightness.in(Value);
+
+    return (reader, writer) -> {
+      applyTo(
+          reader,
+          (i, r, g, b) -> {
+            // Clamp RGB values to keep them in the range [0, 255].
+            // Otherwise, the casts to byte would result in values like 256 wrapping to 0
+
+            writer.setRGB(
+                i,
+                (int) Math.clamp(r * multiplier, 0, 255),
+                (int) Math.clamp(g * multiplier, 0, 255),
+                (int) Math.clamp(b * multiplier, 0, 255));
+          });
+    };
+  }
+
+  /** A pattern that turns off all LEDs. */
+  LEDPattern kOff = solid(Color.kBlack);
+
+  /**
+   * Creates a pattern that displays a single static color along the entire length of the LED strip.
+   *
+   * @param color the color to display
+   * @return the pattern
+   */
+  static LEDPattern solid(Color color) {
+    return (reader, writer) -> {
+      int bufLen = reader.getLength();
+      for (int led = 0; led < bufLen; led++) {
+        writer.setLED(led, color);
+      }
+    };
+  }
+
+  /**
+   * Creates a pattern that works as a mask layer for {@link #mask(LEDPattern)} that illuminates
+   * only the portion of the LED strip corresponding with some progress. The mask pattern will start
+   * from the base and set LEDs to white at a proportion equal to the progress returned by the
+   * function. Some usages for this could be for displaying progress of a flywheel to its target
+   * velocity, progress of a complex autonomous sequence, or the height of an elevator.
+   *
+   * <p>For example, creating a mask for displaying a red-to-blue gradient, starting from the red
+   * end, based on where an elevator is in its range of travel.
+   *
+   * <pre>
+   *   LEDPattern basePattern = gradient(Color.kRed, Color.kBlue);
+   *   LEDPattern progressPattern =
+   *     basePattern.mask(progressMaskLayer(() -> elevator.getHeight() / elevator.maxHeight());
+   * </pre>
+   *
+   * @param progressSupplier the function to call to determine the progress. This should return
+   *     values in the range [0, 1]; any values outside that range will be clamped.
+   * @return the mask pattern
+   */
+  static LEDPattern progressMaskLayer(DoubleSupplier progressSupplier) {
+    return (reader, writer) -> {
+      double progress = Math.clamp(progressSupplier.getAsDouble(), 0, 1);
+
+      int bufLen = reader.getLength();
+      int max = (int) (bufLen * progress);
+
+      for (int led = 0; led < max; led++) {
+        writer.setLED(led, Color.kWhite);
+      }
+
+      for (int led = max; led < bufLen; led++) {
+        writer.setLED(led, Color.kBlack);
+      }
+    };
+  }
+
+  /**
+   * Display a set of colors in steps across the length of the LED strip. No interpolation is done
+   * between colors. Colors are specified by the first LED on the strip to show that color. The last
+   * color in the map will be displayed all the way to the end of the strip. LEDs positioned before
+   * the first specified step will be turned off (you can think of this as if there's a 0 -> black
+   * step by default)
+   *
+   * <pre>
+   *   // Display red from 0-33%, white from 33% - 67%, and blue from 67% to 100%
+   *   steps(Map.of(0.00, Color.kRed, 0.33, Color.kWhite, 0.67, Color.kBlue))
+   *
+   *   // Half off, half on
+   *   steps(Map.of(0.5, Color.kWhite))
+   * </pre>
+   *
+   * @param steps a map of progress to the color to start displaying at that position along the LED
+   *     strip
+   * @return a motionless step pattern
+   */
+  static LEDPattern steps(Map<? extends Number, Color> steps) {
+    if (steps.isEmpty()) {
+      // no colors specified
+      DriverStation.reportWarning("Creating LED steps with no colors!", false);
+      return kOff;
+    }
+
+    if (steps.size() == 1 && steps.keySet().iterator().next().doubleValue() == 0) {
+      // only one color specified, just show a static color
+      DriverStation.reportWarning("Creating LED steps with only one color!", false);
+      return solid(steps.values().iterator().next());
+    }
+
+    return (reader, writer) -> {
+      int bufLen = reader.getLength();
+
+      // precompute relevant positions for this buffer so we don't need to do a check
+      // on every single LED index
+      var stopPositions = new LongToObjectHashMap<Color>();
+      steps.forEach(
+          (progress, color) -> {
+            stopPositions.put((int) Math.floor(progress.doubleValue() * bufLen), color);
+          });
+
+      Color currentColor = Color.kBlack;
+      for (int led = 0; led < bufLen; led++) {
+        currentColor = Objects.requireNonNullElse(stopPositions.get(led), currentColor);
+
+        writer.setLED(led, currentColor);
+      }
+    };
+  }
+
+  /** Types of gradients. */
+  enum GradientType {
+    /**
+     * A continuous gradient, where the gradient wraps around to allow for seamless scrolling
+     * effects.
+     */
+    kContinuous,
+
+    /**
+     * A discontinuous gradient, where the first pixel is set to the first color of the gradient and
+     * the final pixel is set to the last color of the gradient. There is no wrapping effect, so
+     * scrolling effects will display an obvious seam.
+     */
+    kDiscontinuous
+  }
+
+  /**
+   * Creates a pattern that displays a non-animated gradient of colors across the entire length of
+   * the LED strip. Colors are evenly distributed along the full length of the LED strip. The
+   * gradient type is configured with the {@code type} parameter, allowing the gradient to be either
+   * continuous (no seams, good for scrolling effects) or discontinuous (a clear seam is visible,
+   * but the gradient applies to the full length of the LED strip without needing to use some space
+   * for wrapping).
+   *
+   * @param type the type of gradient (continuous or discontinuous)
+   * @param colors the colors to display in the gradient
+   * @return a motionless gradient pattern
+   */
+  static LEDPattern gradient(GradientType type, Color... colors) {
+    if (colors.length == 0) {
+      // Nothing to display
+      DriverStation.reportWarning("Creating a gradient with no colors!", false);
+      return kOff;
+    }
+
+    if (colors.length == 1) {
+      // No gradients with one color
+      DriverStation.reportWarning("Creating a gradient with only one color!", false);
+      return solid(colors[0]);
+    }
+
+    final int numSegments = colors.length;
+
+    return (reader, writer) -> {
+      int bufLen = reader.getLength();
+      int ledsPerSegment =
+          switch (type) {
+            case kContinuous -> bufLen / numSegments;
+            case kDiscontinuous -> (bufLen - 1) / (numSegments - 1);
+          };
+
+      for (int led = 0; led < bufLen; led++) {
+        int colorIndex = (led / ledsPerSegment) % numSegments;
+        int nextColorIndex = (colorIndex + 1) % numSegments;
+        double t = (led / (double) ledsPerSegment) % 1;
+
+        Color color = colors[colorIndex];
+        Color nextColor = colors[nextColorIndex];
+        int gradientColor =
+            Color.lerpRGB(
+                color.red,
+                color.green,
+                color.blue,
+                nextColor.red,
+                nextColor.green,
+                nextColor.blue,
+                t);
+
+        writer.setRGB(
+            led,
+            Color.unpackRGB(gradientColor, Color.RGBChannel.kRed),
+            Color.unpackRGB(gradientColor, Color.RGBChannel.kGreen),
+            Color.unpackRGB(gradientColor, Color.RGBChannel.kBlue));
+      }
+    };
+  }
+
+  /**
+   * Creates an LED pattern that displays a rainbow across the color wheel. The rainbow pattern will
+   * stretch across the entire length of the LED strip.
+   *
+   * @param saturation the saturation of the HSV colors, in [0, 255]
+   * @param value the value of the HSV colors, in [0, 255]
+   * @return the rainbow pattern
+   */
+  static LEDPattern rainbow(int saturation, int value) {
+    return (reader, writer) -> {
+      int bufLen = reader.getLength();
+      for (int i = 0; i < bufLen; i++) {
+        int hue = ((i * 180) / bufLen) % 180;
+        writer.setHSV(i, hue, saturation, value);
+      }
+    };
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/led/LEDReader.java

@@ -0,0 +1,99 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+/** Generic interface for reading data from an LED buffer. */
+public interface LEDReader {
+  /**
+   * Gets the length of the buffer.
+   *
+   * @return the buffer length
+   */
+  int getLength();
+
+  /**
+   * Gets the most recently written color for a particular LED in the buffer.
+   *
+   * @param index the index of the LED
+   * @return the LED color
+   * @throws IndexOutOfBoundsException if the index is negative or greater than {@link #getLength()}
+   */
+  default Color getLED(int index) {
+    return new Color(getRed(index) / 255.0, getGreen(index) / 255.0, getBlue(index) / 255.0);
+  }
+
+  /**
+   * Gets the most recently written color for a particular LED in the buffer.
+   *
+   * @param index the index of the LED
+   * @return the LED color
+   * @throws IndexOutOfBoundsException if the index is negative or greater than {@link #getLength()}
+   */
+  default Color8Bit getLED8Bit(int index) {
+    return new Color8Bit(getRed(index), getGreen(index), getBlue(index));
+  }
+
+  /**
+   * Gets the red channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the red channel, from [0, 255]
+   */
+  int getRed(int index);
+
+  /**
+   * Gets the green channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the green channel, from [0, 255]
+   */
+  int getGreen(int index);
+
+  /**
+   * Gets the blue channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the blue channel, from [0, 255]
+   */
+  int getBlue(int index);
+
+  /**
+   * A functional interface that allows for iteration over an LED buffer without manually writing an
+   * indexed for-loop.
+   */
+  @FunctionalInterface
+  interface IndexedColorIterator {
+    /**
+     * Accepts an index of an LED in the buffer and the red, green, and blue components of the
+     * currently stored color for that LED.
+     *
+     * @param index the index of the LED in the buffer that the red, green, and blue channels
+     *     corresponds to
+     * @param r the value of the red channel of the color currently in the buffer at index {@code i}
+     * @param g the value of the green channel of the color currently in the buffer at index {@code
+     *     i}
+     * @param b the value of the blue channel of the color currently in the buffer at index {@code
+     *     i}
+     */
+    void accept(int index, int r, int g, int b);
+  }
+
+  /**
+   * Iterates over the LEDs in the buffer, starting from index 0. The iterator function is passed
+   * the current index of iteration, along with the values for the red, green, and blue components
+   * of the color written to the LED at that index.
+   *
+   * @param iterator the iterator function to call for each LED in the buffer.
+   */
+  default void forEach(IndexedColorIterator iterator) {
+    int bufLen = getLength();
+    for (int i = 0; i < bufLen; i++) {
+      iterator.accept(i, getRed(i), getGreen(i), getBlue(i));
+    }
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/led/LEDWriter.java

@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+/** Generic interface for writing data to an LED buffer. */
+@FunctionalInterface
+public interface LEDWriter {
+  /**
+   * Sets the RGB value for an LED at a specific index on a LED buffer.
+   *
+   * @param index the index of the LED to write to
+   * @param r the value of the red channel, in [0, 255]
+   * @param g the value of the green channel, in [0, 255]
+   * @param b the value of the blue channel, in [0, 255]
+   */
+  void setRGB(int index, int r, int g, int b);
+
+  /**
+   * Sets a specific led in the buffer.
+   *
+   * @param index the index to write
+   * @param h the h value [0-180)
+   * @param s the s value [0-255]
+   * @param v the v value [0-255]
+   */
+  default void setHSV(int index, int h, int s, int v) {
+    if (s == 0) {
+      setRGB(index, v, v, v);
+      return;
+    }
+
+    int packedRGB = Color.hsvToRgb(h, s, v);
+
+    setRGB(
+        index,
+        Color.unpackRGB(packedRGB, Color.RGBChannel.kRed),
+        Color.unpackRGB(packedRGB, Color.RGBChannel.kGreen),
+        Color.unpackRGB(packedRGB, Color.RGBChannel.kBlue));
+  }
+
+  /**
+   * Sets the RGB value for an LED at a specific index on a LED buffer.
+   *
+   * @param index the index of the LED to write to
+   * @param color the color to set
+   */
+  default void setLED(int index, Color color) {
+    setRGB(index, (int) (color.red * 255), (int) (color.green * 255), (int) (color.blue * 255));
+  }
+
+  /**
+   * Sets the RGB value for an LED at a specific index on a LED buffer.
+   *
+   * @param index the index of the LED to write to
+   * @param color the color to set
+   */
+  default void setLED(int index, Color8Bit color) {
+    setRGB(index, color.red, color.green, color.blue);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/motor/MotorController.java

@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.RobotController;
+
+/** Interface for motor controlling devices. */
+public interface MotorController {
+  /**
+   * Common interface for setting the speed of a motor controller.
+   *
+   * @param speed The speed to set. Value should be between -1.0 and 1.0.
+   */
+  void set(double speed);
+
+  /**
+   * Sets the voltage output of the MotorController. Compensates for the current bus voltage to
+   * ensure that the desired voltage is output even if the battery voltage is below 12V - highly
+   * useful when the voltage outputs are "meaningful" (e.g. they come from a feedforward
+   * calculation).
+   *
+   * <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
+   * properly - unlike the ordinary set function, it is not "set it and forget it."
+   *
+   * @param outputVolts The voltage to output, in Volts.
+   */
+  default void setVoltage(double outputVolts) {
+    set(outputVolts / RobotController.getBatteryVoltage());
+  }
+
+  /**
+   * Sets the voltage output of the MotorController. Compensates for the current bus voltage to
+   * ensure that the desired voltage is output even if the battery voltage is below 12V - highly
+   * useful when the voltage outputs are "meaningful" (e.g. they come from a feedforward
+   * calculation).
+   *
+   * <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
+   * properly - unlike the ordinary set function, it is not "set it and forget it."
+   *
+   * @param outputVoltage The voltage to output.
+   */
+  default void setVoltage(Voltage outputVoltage) {
+    setVoltage(outputVoltage.in(Volts));
+  }
+
+  /**
+   * Common interface for getting the current set speed of a motor controller.
+   *
+   * @return The current set speed. Value is between -1.0 and 1.0.
+   */
+  double get();
+
+  /**
+   * Common interface for inverting direction of a motor controller.
+   *
+   * @param isInverted The state of inversion true is inverted.
+   */
+  void setInverted(boolean isInverted);
+
+  /**
+   * Common interface for returning if a motor controller is in the inverted state or not.
+   *
+   * @return isInverted The state of the inversion true is inverted.
+   */
+  boolean getInverted();
+
+  /** Disable the motor controller. */
+  void disable();
+
+  /**
+   * Stops motor movement. Motor can be moved again by calling set without having to re-enable the
+   * motor.
+   */
+  void stopMotor();
+}

wpilibj/src/main/java/org/wpilib/hardware/motor/MotorControllerGroup.java

@@ -0,0 +1,116 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.Arrays;
+
+/**
+ * Allows multiple {@link MotorController} objects to be linked together.
+ *
+ * @deprecated Use {@link PWMMotorController#addFollower(PWMMotorController)} or if using CAN motor
+ *     controllers, use their method of following.
+ */
+@SuppressWarnings("removal")
+@Deprecated(forRemoval = true, since = "2024")
+public class MotorControllerGroup implements MotorController, Sendable, AutoCloseable {
+  private boolean m_isInverted;
+  private final MotorController[] m_motorControllers;
+  private static int instances;
+
+  /**
+   * Create a new MotorControllerGroup with the provided MotorControllers.
+   *
+   * @param motorController The first MotorController to add
+   * @param motorControllers The MotorControllers to add
+   */
+  @SuppressWarnings("this-escape")
+  public MotorControllerGroup(
+      MotorController motorController, MotorController... motorControllers) {
+    m_motorControllers = new MotorController[motorControllers.length + 1];
+    m_motorControllers[0] = motorController;
+    System.arraycopy(motorControllers, 0, m_motorControllers, 1, motorControllers.length);
+    init();
+  }
+
+  /**
+   * Create a new MotorControllerGroup with the provided MotorControllers.
+   *
+   * @param motorControllers The MotorControllers to add.
+   */
+  @SuppressWarnings("this-escape")
+  public MotorControllerGroup(MotorController[] motorControllers) {
+    m_motorControllers = Arrays.copyOf(motorControllers, motorControllers.length);
+    init();
+  }
+
+  private void init() {
+    for (MotorController controller : m_motorControllers) {
+      SendableRegistry.addChild(this, controller);
+    }
+    instances++;
+    SendableRegistry.add(this, "MotorControllerGroup", instances);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  @Override
+  public void set(double speed) {
+    for (MotorController motorController : m_motorControllers) {
+      motorController.set(m_isInverted ? -speed : speed);
+    }
+  }
+
+  @Override
+  public void setVoltage(double outputVolts) {
+    for (MotorController motorController : m_motorControllers) {
+      motorController.setVoltage(m_isInverted ? -outputVolts : outputVolts);
+    }
+  }
+
+  @Override
+  public double get() {
+    if (m_motorControllers.length > 0) {
+      return m_motorControllers[0].get() * (m_isInverted ? -1 : 1);
+    }
+    return 0.0;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  @Override
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  @Override
+  public void disable() {
+    for (MotorController motorController : m_motorControllers) {
+      motorController.disable();
+    }
+  }
+
+  @Override
+  public void stopMotor() {
+    for (MotorController motorController : m_motorControllers) {
+      motorController.stopMotor();
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Motor Controller");
+    builder.setActuator(true);
+    builder.addDoubleProperty("Value", this::get, this::set);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/motor/MotorSafety.java

@@ -0,0 +1,198 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.ControlWord;
+import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.util.WPIUtilJNI;
+import java.util.LinkedHashSet;
+import java.util.Set;
+
+/**
+ * The Motor Safety feature acts as a watchdog timer for an individual motor. It operates by
+ * maintaining a timer that tracks how long it has been since the feed() method has been called for
+ * that actuator. Code in the Driver Station class initiates a comparison of these timers to the
+ * timeout values for any actuator with safety enabled every 5 received packets (100ms nominal).
+ *
+ * <p>The subclass should call feed() whenever the motor value is updated.
+ */
+public abstract class MotorSafety {
+  private static final double kDefaultSafetyExpiration = 0.1;
+
+  private double m_expiration = kDefaultSafetyExpiration;
+  private boolean m_enabled;
+  private double m_stopTime = Timer.getFPGATimestamp();
+  private final Object m_thisMutex = new Object();
+  private static final Set<MotorSafety> m_instanceList = new LinkedHashSet<>();
+  private static final Object m_listMutex = new Object();
+  private static Thread m_safetyThread;
+
+  @SuppressWarnings("PMD.AssignmentInOperand")
+  private static void threadMain() {
+    int event = WPIUtilJNI.createEvent(false, false);
+    DriverStationJNI.provideNewDataEventHandle(event);
+    ControlWord controlWord = new ControlWord();
+
+    int safetyCounter = 0;
+    while (true) {
+      boolean timedOut;
+      try {
+        timedOut = WPIUtilJNI.waitForObjectTimeout(event, 0.1);
+      } catch (InterruptedException e) {
+        DriverStationJNI.removeNewDataEventHandle(event);
+        WPIUtilJNI.destroyEvent(event);
+        Thread.currentThread().interrupt();
+        return;
+      }
+      if (!timedOut) {
+        DriverStationJNI.getControlWord(controlWord);
+        if (!(controlWord.getEnabled() && controlWord.getDSAttached())) {
+          safetyCounter = 0;
+        }
+        if (++safetyCounter >= 4) {
+          checkMotors();
+          safetyCounter = 0;
+        }
+      } else {
+        safetyCounter = 0;
+      }
+    }
+  }
+
+  /** MotorSafety constructor. */
+  @SuppressWarnings("this-escape")
+  public MotorSafety() {
+    synchronized (m_listMutex) {
+      m_instanceList.add(this);
+      if (m_safetyThread == null) {
+        m_safetyThread = new Thread(MotorSafety::threadMain, "MotorSafety Thread");
+        m_safetyThread.setDaemon(true);
+        m_safetyThread.start();
+      }
+    }
+  }
+
+  /**
+   * Feed the motor safety object.
+   *
+   * <p>Resets the timer on this object that is used to do the timeouts.
+   */
+  public void feed() {
+    synchronized (m_thisMutex) {
+      m_stopTime = Timer.getFPGATimestamp() + m_expiration;
+    }
+  }
+
+  /**
+   * Set the expiration time for the corresponding motor safety object.
+   *
+   * @param expirationTime The timeout value in seconds.
+   */
+  public void setExpiration(double expirationTime) {
+    synchronized (m_thisMutex) {
+      m_expiration = expirationTime;
+    }
+  }
+
+  /**
+   * Retrieve the timeout value for the corresponding motor safety object.
+   *
+   * @return the timeout value in seconds.
+   */
+  public double getExpiration() {
+    synchronized (m_thisMutex) {
+      return m_expiration;
+    }
+  }
+
+  /**
+   * Determine of the motor is still operating or has timed out.
+   *
+   * @return a true value if the motor is still operating normally and hasn't timed out.
+   */
+  public boolean isAlive() {
+    synchronized (m_thisMutex) {
+      return !m_enabled || m_stopTime > Timer.getFPGATimestamp();
+    }
+  }
+
+  /**
+   * Check if this motor has exceeded its timeout. This method is called periodically to determine
+   * if this motor has exceeded its timeout value. If it has, the stop method is called, and the
+   * motor is shut down until its value is updated again.
+   */
+  public void check() {
+    boolean enabled;
+    double stopTime;
+
+    synchronized (m_thisMutex) {
+      enabled = m_enabled;
+      stopTime = m_stopTime;
+    }
+
+    if (!enabled || RobotState.isDisabled() || RobotState.isTest()) {
+      return;
+    }
+
+    if (stopTime < Timer.getFPGATimestamp()) {
+      DriverStation.reportError(
+          getDescription()
+              + "... Output not updated often enough. See https://docs.wpilib.org/motorsafety for more information.",
+          false);
+
+      stopMotor();
+    }
+  }
+
+  /**
+   * Enable/disable motor safety for this device.
+   *
+   * <p>Turn on and off the motor safety option for this PWM object.
+   *
+   * @param enabled True if motor safety is enforced for this object
+   */
+  public void setSafetyEnabled(boolean enabled) {
+    synchronized (m_thisMutex) {
+      m_enabled = enabled;
+    }
+  }
+
+  /**
+   * Return the state of the motor safety enabled flag.
+   *
+   * <p>Return if the motor safety is currently enabled for this device.
+   *
+   * @return True if motor safety is enforced for this device
+   */
+  public boolean isSafetyEnabled() {
+    synchronized (m_thisMutex) {
+      return m_enabled;
+    }
+  }
+
+  /**
+   * Check the motors to see if any have timed out.
+   *
+   * <p>This static method is called periodically to poll all the motors and stop any that have
+   * timed out.
+   */
+  public static void checkMotors() {
+    synchronized (m_listMutex) {
+      for (MotorSafety elem : m_instanceList) {
+        elem.check();
+      }
+    }
+  }
+
+  /** Called to stop the motor when the timeout expires. */
+  public abstract void stopMotor();
+
+  /**
+   * Returns a description to print when an error occurs.
+   *
+   * @return Description to print when an error occurs.
+   */
+  public abstract String getDescription();
+}

wpilibj/src/main/java/org/wpilib/hardware/motor/PWMMotorController.java

@@ -0,0 +1,286 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.MotorSafety;
+import edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj.RobotController;
+import java.util.ArrayList;
+
+/** Common base class for all PWM Motor Controllers. */
+@SuppressWarnings("removal")
+public abstract class PWMMotorController extends MotorSafety
+    implements MotorController, Sendable, AutoCloseable {
+  private boolean m_isInverted;
+  private final ArrayList<PWMMotorController> m_followers = new ArrayList<>();
+
+  /** PWM instances for motor controller. */
+  protected PWM m_pwm;
+
+  private SimDevice m_simDevice;
+  private SimDouble m_simSpeed;
+
+  private boolean m_eliminateDeadband;
+  private int m_minPwm;
+  private int m_deadbandMinPwm;
+  private int m_centerPwm;
+  private int m_deadbandMaxPwm;
+  private int m_maxPwm;
+
+  /**
+   * Constructor.
+   *
+   * @param name Name to use for SendableRegistry
+   * @param channel The PWM channel that the controller is attached to. 0-9 are on-board, 10-19 are
+   *     on the MXP port
+   */
+  @SuppressWarnings("this-escape")
+  protected PWMMotorController(final String name, final int channel) {
+    m_pwm = new PWM(channel, false);
+    SendableRegistry.add(this, name, channel);
+
+    m_simDevice = SimDevice.create("PWMMotorController", channel);
+    if (m_simDevice != null) {
+      m_simSpeed = m_simDevice.createDouble("Speed", Direction.kOutput, 0.0);
+      m_pwm.setSimDevice(m_simDevice);
+    }
+  }
+
+  /** Free the resource associated with the PWM channel and set the value to 0. */
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    m_pwm.close();
+
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+      m_simSpeed = null;
+    }
+  }
+
+  private int getMinPositivePwm() {
+    if (m_eliminateDeadband) {
+      return m_deadbandMaxPwm;
+    } else {
+      return m_centerPwm + 1;
+    }
+  }
+
+  private int getMaxNegativePwm() {
+    if (m_eliminateDeadband) {
+      return m_deadbandMinPwm;
+    } else {
+      return m_centerPwm - 1;
+    }
+  }
+
+  private int getPositiveScaleFactor() {
+    return m_maxPwm - getMinPositivePwm();
+  }
+
+  private int getNegativeScaleFactor() {
+    return getMaxNegativePwm() - m_minPwm;
+  }
+
+  /**
+   * Takes a speed from -1 to 1, and outputs it in the microsecond format.
+   *
+   * @param speed the speed to output
+   */
+  protected final void setSpeed(double speed) {
+    if (Double.isFinite(speed)) {
+      speed = Math.clamp(speed, -1.0, 1.0);
+    } else {
+      speed = 0.0;
+    }
+
+    if (m_simSpeed != null) {
+      m_simSpeed.set(speed);
+    }
+
+    int rawValue;
+    if (speed == 0.0) {
+      rawValue = m_centerPwm;
+    } else if (speed > 0.0) {
+      rawValue = (int) Math.round(speed * getPositiveScaleFactor()) + getMinPositivePwm();
+    } else {
+      rawValue = (int) Math.round(speed * getNegativeScaleFactor()) + getMaxNegativePwm();
+    }
+
+    m_pwm.setPulseTimeMicroseconds(rawValue);
+  }
+
+  /**
+   * Gets the speed from -1 to 1, from the currently set pulse time.
+   *
+   * @return motor controller speed
+   */
+  protected final double getSpeed() {
+    int rawValue = m_pwm.getPulseTimeMicroseconds();
+
+    if (rawValue == 0) {
+      return 0.0;
+    } else if (rawValue > m_maxPwm) {
+      return 1.0;
+    } else if (rawValue < m_minPwm) {
+      return -1.0;
+    } else if (rawValue > getMinPositivePwm()) {
+      return (rawValue - getMinPositivePwm()) / (double) getPositiveScaleFactor();
+    } else if (rawValue < getMaxNegativePwm()) {
+      return (rawValue - getMaxNegativePwm()) / (double) getNegativeScaleFactor();
+    } else {
+      return 0.0;
+    }
+  }
+
+  /**
+   * Sets the bounds in microseconds for the controller.
+   *
+   * @param maxPwm maximum
+   * @param deadbandMaxPwm deadband max
+   * @param centerPwm center
+   * @param deadbandMinPwm deadmand min
+   * @param minPwm minimum
+   */
+  protected final void setBoundsMicroseconds(
+      int maxPwm, int deadbandMaxPwm, int centerPwm, int deadbandMinPwm, int minPwm) {
+    m_maxPwm = maxPwm;
+    m_deadbandMaxPwm = deadbandMaxPwm;
+    m_centerPwm = centerPwm;
+    m_deadbandMinPwm = deadbandMinPwm;
+    m_minPwm = minPwm;
+  }
+
+  /**
+   * Set the PWM value.
+   *
+   * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
+   * FPGA.
+   *
+   * @param speed The speed value between -1.0 and 1.0 to set.
+   */
+  @Override
+  public void set(double speed) {
+    if (m_isInverted) {
+      speed = -speed;
+    }
+    setSpeed(speed);
+
+    for (var follower : m_followers) {
+      follower.set(speed);
+    }
+
+    feed();
+  }
+
+  /**
+   * Get the recently set value of the PWM. This value is affected by the inversion property.
+   *
+   * @return The most recently set value for the PWM between -1.0 and 1.0.
+   */
+  @Override
+  public double get() {
+    return getSpeed() * (m_isInverted ? -1.0 : 1.0);
+  }
+
+  /**
+   * Gets the voltage output of the motor controller, nominally between -12 V and 12 V.
+   *
+   * @return The voltage of the motor controller, nominally between -12 V and 12 V.
+   */
+  public double getVoltage() {
+    return get() * RobotController.getBatteryVoltage();
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  @Override
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  @Override
+  public void disable() {
+    m_pwm.setDisabled();
+
+    if (m_simSpeed != null) {
+      m_simSpeed.set(0.0);
+    }
+
+    for (var follower : m_followers) {
+      follower.disable();
+    }
+  }
+
+  @Override
+  public void stopMotor() {
+    // Don't use set(0) as that will feed the watch kitty
+    m_pwm.setPulseTimeMicroseconds(0);
+
+    for (var follower : m_followers) {
+      follower.stopMotor();
+    }
+  }
+
+  @Override
+  public String getDescription() {
+    return "PWM " + getChannel();
+  }
+
+  /**
+   * Gets the backing PWM handle.
+   *
+   * @return The pwm handle.
+   */
+  public int getPwmHandle() {
+    return m_pwm.getHandle();
+  }
+
+  /**
+   * Gets the PWM channel number.
+   *
+   * @return The channel number.
+   */
+  public int getChannel() {
+    return m_pwm.getChannel();
+  }
+
+  /**
+   * Optionally eliminate the deadband from a motor controller.
+   *
+   * @param eliminateDeadband If true, set the motor curve for the motor controller to eliminate the
+   *     deadband in the middle of the range. Otherwise, keep the full range without modifying any
+   *     values.
+   */
+  public void enableDeadbandElimination(boolean eliminateDeadband) {
+    m_eliminateDeadband = eliminateDeadband;
+  }
+
+  /**
+   * Make the given PWM motor controller follow the output of this one.
+   *
+   * @param follower The motor controller follower.
+   */
+  public void addFollower(PWMMotorController follower) {
+    m_followers.add(follower);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Motor Controller");
+    builder.setActuator(true);
+    builder.addDoubleProperty("Value", this::get, this::set);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/pneumatic/Compressor.java

@@ -0,0 +1,205 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Class for operating a compressor connected to a pneumatics module. The module will automatically
+ * run in closed loop mode by default whenever a {@link Solenoid} object is created. For most cases,
+ * a Compressor object does not need to be instantiated or used in a robot program. This class is
+ * only required in cases where the robot program needs a more detailed status of the compressor or
+ * to enable/disable closed loop control.
+ *
+ * <p>Note: you cannot operate the compressor directly from this class as doing so would circumvent
+ * the safety provided by using the pressure switch and closed loop control. You can only turn off
+ * closed loop control, thereby stopping the compressor from operating.
+ */
+public class Compressor implements Sendable, AutoCloseable {
+  private PneumaticsBase m_module;
+  private PneumaticsModuleType m_moduleType;
+
+  /**
+   * Constructs a compressor for a specified module and type.
+   *
+   * @param busId The bus ID
+   * @param module The module ID to use.
+   * @param moduleType The module type to use.
+   */
+  @SuppressWarnings("this-escape")
+  public Compressor(int busId, int module, PneumaticsModuleType moduleType) {
+    m_module = PneumaticsBase.getForType(busId, module, moduleType);
+    m_moduleType = moduleType;
+
+    if (!m_module.reserveCompressor()) {
+      m_module.close();
+      throw new AllocationException("Compressor already allocated");
+    }
+
+    m_module.enableCompressorDigital();
+
+    m_module.reportUsage("Compressor", "");
+    SendableRegistry.add(this, "Compressor", module);
+  }
+
+  /**
+   * Constructs a compressor for a default module and specified type.
+   *
+   * @param busId The bus ID
+   * @param moduleType The module type to use.
+   */
+  public Compressor(int busId, PneumaticsModuleType moduleType) {
+    this(busId, PneumaticsBase.getDefaultForType(moduleType), moduleType);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    m_module.unreserveCompressor();
+    m_module.close();
+    m_module = null;
+  }
+
+  /**
+   * Returns whether the compressor is active or not.
+   *
+   * @return true if the compressor is on - otherwise false.
+   */
+  public boolean isEnabled() {
+    return m_module.getCompressor();
+  }
+
+  /**
+   * Returns the state of the pressure switch.
+   *
+   * @return True if pressure switch indicates that the system is not full, otherwise false.
+   */
+  public boolean getPressureSwitchValue() {
+    return m_module.getPressureSwitch();
+  }
+
+  /**
+   * Get the current drawn by the compressor.
+   *
+   * @return Current drawn by the compressor in amps.
+   */
+  public double getCurrent() {
+    return m_module.getCompressorCurrent();
+  }
+
+  /**
+   * If supported by the device, returns the analog input voltage (on channel 0).
+   *
+   * <p>This function is only supported by the REV PH. On CTRE PCM, this will return 0.
+   *
+   * @return The analog input voltage, in volts.
+   */
+  public double getAnalogVoltage() {
+    return m_module.getAnalogVoltage(0);
+  }
+
+  /**
+   * If supported by the device, returns the pressure (in PSI) read by the analog pressure sensor
+   * (on channel 0).
+   *
+   * <p>This function is only supported by the REV PH with the REV Analog Pressure Sensor. On CTRE
+   * PCM, this will return 0.
+   *
+   * @return The pressure (in PSI) read by the analog pressure sensor.
+   */
+  public double getPressure() {
+    return m_module.getPressure(0);
+  }
+
+  /** Disable the compressor. */
+  public void disable() {
+    m_module.disableCompressor();
+  }
+
+  /**
+   * Enables the compressor in digital mode using the digital pressure switch. The compressor will
+   * turn on when the pressure switch indicates that the system is not full, and will turn off when
+   * the pressure switch indicates that the system is full.
+   */
+  public void enableDigital() {
+    m_module.enableCompressorDigital();
+  }
+
+  /**
+   * If supported by the device, enables the compressor in analog mode. This mode uses an analog
+   * pressure sensor connected to analog channel 0 to cycle the compressor. The compressor will turn
+   * on when the pressure drops below {@code minPressure} and will turn off when the pressure
+   * reaches {@code maxPressure}. This mode is only supported by the REV PH with the REV Analog
+   * Pressure Sensor connected to analog channel 0.
+   *
+   * <p>On CTRE PCM, this will enable digital control.
+   *
+   * @param minPressure The minimum pressure in PSI. The compressor will turn on when the pressure
+   *     drops below this value.
+   * @param maxPressure The maximum pressure in PSI. The compressor will turn off when the pressure
+   *     reaches this value.
+   */
+  public void enableAnalog(double minPressure, double maxPressure) {
+    m_module.enableCompressorAnalog(minPressure, maxPressure);
+  }
+
+  /**
+   * If supported by the device, enables the compressor in hybrid mode. This mode uses both a
+   * digital pressure switch and an analog pressure sensor connected to analog channel 0 to cycle
+   * the compressor. This mode is only supported by the REV PH with the REV Analog Pressure Sensor
+   * connected to analog channel 0.
+   *
+   * <p>The compressor will turn on when <i>both</i>:
+   *
+   * <ul>
+   *   <li>The digital pressure switch indicates the system is not full AND
+   *   <li>The analog pressure sensor indicates that the pressure in the system is below the
+   *       specified minimum pressure.
+   * </ul>
+   *
+   * <p>The compressor will turn off when <i>either</i>:
+   *
+   * <ul>
+   *   <li>The digital pressure switch is disconnected or indicates that the system is full OR
+   *   <li>The pressure detected by the analog sensor is greater than the specified maximum
+   *       pressure.
+   * </ul>
+   *
+   * <p>On CTRE PCM, this will enable digital control.
+   *
+   * @param minPressure The minimum pressure in PSI. The compressor will turn on when the pressure
+   *     drops below this value and the pressure switch indicates that the system is not full.
+   * @param maxPressure The maximum pressure in PSI. The compressor will turn off when the pressure
+   *     reaches this value or the pressure switch is disconnected or indicates that the system is
+   *     full.
+   */
+  public void enableHybrid(double minPressure, double maxPressure) {
+    m_module.enableCompressorHybrid(minPressure, maxPressure);
+  }
+
+  /**
+   * Returns the active compressor configuration.
+   *
+   * @return The active compressor configuration.
+   */
+  public CompressorConfigType getConfigType() {
+    return m_module.getCompressorConfigType();
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Compressor");
+    builder.addBooleanProperty("Enabled", this::isEnabled, null);
+    builder.addBooleanProperty("Pressure switch", this::getPressureSwitchValue, null);
+    builder.addDoubleProperty("Current (A)", this::getCurrent, null);
+    if (m_moduleType == PneumaticsModuleType.REVPH) { // These are not supported by the CTRE PCM
+      builder.addDoubleProperty("Analog Voltage", this::getAnalogVoltage, null);
+      builder.addDoubleProperty("Pressure (PSI)", this::getPressure, null);
+    }
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/pneumatic/CompressorConfigType.java

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.REVPHJNI;
+
+/** Compressor config type. */
+public enum CompressorConfigType {
+  /** Disabled. */
+  Disabled(REVPHJNI.COMPRESSOR_CONFIG_TYPE_DISABLED),
+  /** Digital. */
+  Digital(REVPHJNI.COMPRESSOR_CONFIG_TYPE_DIGITAL),
+  /** Analog. */
+  Analog(REVPHJNI.COMPRESSOR_CONFIG_TYPE_ANALOG),
+  /** Hybrid. */
+  Hybrid(REVPHJNI.COMPRESSOR_CONFIG_TYPE_HYBRID);
+
+  /** CompressorConfigType value. */
+  public final int value;
+
+  CompressorConfigType(int value) {
+    this.value = value;
+  }
+
+  /**
+   * Gets a type from an int value.
+   *
+   * @param value int value
+   * @return type
+   */
+  public static CompressorConfigType fromValue(int value) {
+    return switch (value) {
+      case REVPHJNI.COMPRESSOR_CONFIG_TYPE_HYBRID -> Hybrid;
+      case REVPHJNI.COMPRESSOR_CONFIG_TYPE_ANALOG -> Analog;
+      case REVPHJNI.COMPRESSOR_CONFIG_TYPE_DIGITAL -> Digital;
+      default -> Disabled;
+    };
+  }
+
+  /**
+   * Returns the CompressorConfigType's value.
+   *
+   * @return The CompressorConfigType's value.
+   */
+  public int getValue() {
+    return value;
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/pneumatic/DoubleSolenoid.java

@@ -0,0 +1,233 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * DoubleSolenoid class for running 2 channels of high voltage Digital Output on the pneumatics
+ * module.
+ *
+ * <p>The DoubleSolenoid class is typically used for pneumatics solenoids that have two positions
+ * controlled by two separate channels.
+ */
+public class DoubleSolenoid implements Sendable, AutoCloseable {
+  /** Possible values for a DoubleSolenoid. */
+  public enum Value {
+    /** Off position. */
+    kOff,
+    /** Forward position. */
+    kForward,
+    /** Reverse position. */
+    kReverse
+  }
+
+  private final int m_forwardMask; // The mask for the forward channel.
+  private final int m_reverseMask; // The mask for the reverse channel.
+  private final int m_mask; // The channel mask
+  private PneumaticsBase m_module;
+  private final int m_forwardChannel;
+  private final int m_reverseChannel;
+
+  /**
+   * Constructs a double solenoid for a default module of a specific module type.
+   *
+   * @param busId The bus ID
+   * @param moduleType The module type to use.
+   * @param forwardChannel The forward channel on the module to control.
+   * @param reverseChannel The reverse channel on the module to control.
+   */
+  public DoubleSolenoid(
+      final int busId,
+      final PneumaticsModuleType moduleType,
+      final int forwardChannel,
+      final int reverseChannel) {
+    this(
+        busId,
+        PneumaticsBase.getDefaultForType(moduleType),
+        moduleType,
+        forwardChannel,
+        reverseChannel);
+  }
+
+  /**
+   * Constructs a double solenoid for a specified module of a specific module type.
+   *
+   * @param busId The bus ID
+   * @param module The module of the solenoid module to use.
+   * @param moduleType The module type to use.
+   * @param forwardChannel The forward channel on the module to control.
+   * @param reverseChannel The reverse channel on the module to control.
+   */
+  @SuppressWarnings({"PMD.UseTryWithResources", "this-escape"})
+  public DoubleSolenoid(
+      final int busId,
+      final int module,
+      final PneumaticsModuleType moduleType,
+      final int forwardChannel,
+      final int reverseChannel) {
+    m_module = PneumaticsBase.getForType(busId, module, moduleType);
+    boolean allocatedSolenoids = false;
+    boolean successfulCompletion = false;
+
+    m_forwardChannel = forwardChannel;
+    m_reverseChannel = reverseChannel;
+
+    m_forwardMask = 1 << forwardChannel;
+    m_reverseMask = 1 << reverseChannel;
+    m_mask = m_forwardMask | m_reverseMask;
+
+    try {
+      if (!m_module.checkSolenoidChannel(forwardChannel)) {
+        throw new IllegalArgumentException("Channel " + forwardChannel + " out of range");
+      }
+
+      if (!m_module.checkSolenoidChannel(reverseChannel)) {
+        throw new IllegalArgumentException("Channel " + reverseChannel + " out of range");
+      }
+
+      int allocMask = m_module.checkAndReserveSolenoids(m_mask);
+      if (allocMask != 0) {
+        if (allocMask == m_mask) {
+          throw new AllocationException(
+              "Channels " + forwardChannel + " and " + reverseChannel + " already allocated");
+        } else if (allocMask == m_forwardMask) {
+          throw new AllocationException("Channel " + forwardChannel + " already allocated");
+        } else {
+          throw new AllocationException("Channel " + reverseChannel + " already allocated");
+        }
+      }
+      allocatedSolenoids = true;
+
+      m_module.reportUsage(
+          "Solenoid[" + forwardChannel + "," + reverseChannel + "]", "DoubleSolenoid");
+      SendableRegistry.add(this, "DoubleSolenoid", m_module.getModuleNumber(), forwardChannel);
+      successfulCompletion = true;
+    } finally {
+      if (!successfulCompletion) {
+        if (allocatedSolenoids) {
+          m_module.unreserveSolenoids(m_mask);
+        }
+        m_module.close();
+      }
+    }
+  }
+
+  @Override
+  public synchronized void close() {
+    SendableRegistry.remove(this);
+    m_module.unreserveSolenoids(m_mask);
+    m_module.close();
+    m_module = null;
+  }
+
+  /**
+   * Set the value of a solenoid.
+   *
+   * @param value The value to set (Off, Forward, Reverse)
+   */
+  public void set(final Value value) {
+    int setValue =
+        switch (value) {
+          case kOff -> 0;
+          case kForward -> m_forwardMask;
+          case kReverse -> m_reverseMask;
+        };
+
+    m_module.setSolenoids(m_mask, setValue);
+  }
+
+  /**
+   * Read the current value of the solenoid.
+   *
+   * @return The current value of the solenoid.
+   */
+  public Value get() {
+    int values = m_module.getSolenoids();
+
+    if ((values & m_forwardMask) != 0) {
+      return Value.kForward;
+    } else if ((values & m_reverseMask) != 0) {
+      return Value.kReverse;
+    } else {
+      return Value.kOff;
+    }
+  }
+
+  /**
+   * Toggle the value of the solenoid.
+   *
+   * <p>If the solenoid is set to forward, it'll be set to reverse. If the solenoid is set to
+   * reverse, it'll be set to forward. If the solenoid is set to off, nothing happens.
+   */
+  public void toggle() {
+    Value value = get();
+
+    if (value == Value.kForward) {
+      set(Value.kReverse);
+    } else if (value == Value.kReverse) {
+      set(Value.kForward);
+    }
+  }
+
+  /**
+   * Get the forward channel.
+   *
+   * @return the forward channel.
+   */
+  public int getFwdChannel() {
+    return m_forwardChannel;
+  }
+
+  /**
+   * Get the reverse channel.
+   *
+   * @return the reverse channel.
+   */
+  public int getRevChannel() {
+    return m_reverseChannel;
+  }
+
+  /**
+   * Check if the forward solenoid is Disabled. If a solenoid is shorted, it is added to the
+   * DisabledList and disabled until power cycle, or until faults are cleared.
+   *
+   * @return If solenoid is disabled due to short.
+   */
+  public boolean isFwdSolenoidDisabled() {
+    return (m_module.getSolenoidDisabledList() & m_forwardMask) != 0;
+  }
+
+  /**
+   * Check if the reverse solenoid is Disabled. If a solenoid is shorted, it is added to the
+   * DisabledList and disabled until power cycle, or until faults are cleared.
+   *
+   * @return If solenoid is disabled due to short.
+   */
+  public boolean isRevSolenoidDisabled() {
+    return (m_module.getSolenoidDisabledList() & m_reverseMask) != 0;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Double Solenoid");
+    builder.setActuator(true);
+    builder.addStringProperty(
+        "Value",
+        () -> get().name().substring(1),
+        value -> {
+          if ("Forward".equals(value)) {
+            set(Value.kForward);
+          } else if ("Reverse".equals(value)) {
+            set(Value.kReverse);
+          } else {
+            set(Value.kOff);
+          }
+        });
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/pneumatic/PneumaticHub.java

@@ -0,0 +1,432 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.PortsJNI;
+import edu.wpi.first.hal.REVPHFaults;
+import edu.wpi.first.hal.REVPHJNI;
+import edu.wpi.first.hal.REVPHStickyFaults;
+import edu.wpi.first.hal.REVPHVersion;
+import java.util.HashMap;
+import java.util.Map;
+
+/** Module class for controlling a REV Robotics Pneumatic Hub. */
+public class PneumaticHub implements PneumaticsBase {
+  private static class DataStore implements AutoCloseable {
+    public final int m_module;
+    public final int m_handle;
+    private final int m_busId;
+    private int m_refCount;
+    private int m_reservedMask;
+    private boolean m_compressorReserved;
+    public final int[] m_oneShotDurMs = new int[PortsJNI.getNumREVPHChannels()];
+    private final Object m_reserveLock = new Object();
+
+    DataStore(int busId, int module) {
+      m_handle = REVPHJNI.initialize(busId, module);
+      m_module = module;
+      m_busId = busId;
+      m_handleMaps[busId].put(module, this);
+
+      final REVPHVersion version = REVPHJNI.getVersion(m_handle);
+      final String fwVersion =
+          version.firmwareMajor + "." + version.firmwareMinor + "." + version.firmwareFix;
+
+      // Check PH firmware version
+      if (version.firmwareMajor > 0 && version.firmwareMajor < 22) {
+        throw new IllegalStateException(
+            "The Pneumatic Hub has firmware version "
+                + fwVersion
+                + ", and must be updated to version 2022.0.0 or later "
+                + "using the REV Hardware Client.");
+      }
+    }
+
+    @Override
+    public void close() {
+      REVPHJNI.free(m_handle);
+      m_handleMaps[m_busId].remove(m_module);
+    }
+
+    public void addRef() {
+      m_refCount++;
+    }
+
+    public void removeRef() {
+      m_refCount--;
+      if (m_refCount == 0) {
+        this.close();
+      }
+    }
+  }
+
+  @SuppressWarnings({"unchecked", "rawtypes"})
+  private static final Map<Integer, DataStore>[] m_handleMaps =
+      (Map<Integer, DataStore>[]) new Map[PortsJNI.getNumCanBuses()];
+
+  private static final Object m_handleLock = new Object();
+
+  private static DataStore getForModule(int busId, int module) {
+    synchronized (m_handleLock) {
+      Map<Integer, DataStore> handleMap = m_handleMaps[busId];
+      if (handleMap == null) {
+        handleMap = new HashMap<>();
+        m_handleMaps[busId] = handleMap;
+      }
+
+      DataStore pcm = handleMap.get(module);
+      if (pcm == null) {
+        pcm = new DataStore(busId, module);
+      }
+      pcm.addRef();
+      return pcm;
+    }
+  }
+
+  private static void freeModule(DataStore store) {
+    synchronized (m_handleLock) {
+      store.removeRef();
+    }
+  }
+
+  /** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
+  private static double voltsToPsi(double sensorVoltage, double supplyVoltage) {
+    return 250 * (sensorVoltage / supplyVoltage) - 25;
+  }
+
+  /** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
+  private static double psiToVolts(double pressure, double supplyVoltage) {
+    return supplyVoltage * (0.004 * pressure + 0.1);
+  }
+
+  private final DataStore m_dataStore;
+  private final int m_handle;
+
+  /**
+   * Constructs a PneumaticHub with the default ID (1).
+   *
+   * @param busId The bus ID
+   */
+  public PneumaticHub(int busId) {
+    this(busId, SensorUtil.getDefaultREVPHModule());
+  }
+
+  /**
+   * Constructs a PneumaticHub.
+   *
+   * @param busId The bus ID
+   * @param module module number to construct
+   */
+  public PneumaticHub(int busId, int module) {
+    m_dataStore = getForModule(busId, module);
+    m_handle = m_dataStore.m_handle;
+  }
+
+  @Override
+  public void close() {
+    freeModule(m_dataStore);
+  }
+
+  @Override
+  public boolean getCompressor() {
+    return REVPHJNI.getCompressor(m_handle);
+  }
+
+  @Override
+  public CompressorConfigType getCompressorConfigType() {
+    return CompressorConfigType.fromValue(REVPHJNI.getCompressorConfig(m_handle));
+  }
+
+  @Override
+  public boolean getPressureSwitch() {
+    return REVPHJNI.getPressureSwitch(m_handle);
+  }
+
+  @Override
+  public double getCompressorCurrent() {
+    return REVPHJNI.getCompressorCurrent(m_handle);
+  }
+
+  @Override
+  public void setSolenoids(int mask, int values) {
+    REVPHJNI.setSolenoids(m_handle, mask, values);
+  }
+
+  @Override
+  public int getSolenoids() {
+    return REVPHJNI.getSolenoids(m_handle);
+  }
+
+  @Override
+  public int getModuleNumber() {
+    return m_dataStore.m_module;
+  }
+
+  @Override
+  public void fireOneShot(int index) {
+    REVPHJNI.fireOneShot(m_handle, index, m_dataStore.m_oneShotDurMs[index]);
+  }
+
+  @Override
+  public void setOneShotDuration(int index, int durMs) {
+    m_dataStore.m_oneShotDurMs[index] = durMs;
+  }
+
+  @Override
+  public boolean checkSolenoidChannel(int channel) {
+    return REVPHJNI.checkSolenoidChannel(channel);
+  }
+
+  @Override
+  public int checkAndReserveSolenoids(int mask) {
+    synchronized (m_dataStore.m_reserveLock) {
+      if ((m_dataStore.m_reservedMask & mask) != 0) {
+        return m_dataStore.m_reservedMask & mask;
+      }
+      m_dataStore.m_reservedMask |= mask;
+      return 0;
+    }
+  }
+
+  @Override
+  public void unreserveSolenoids(int mask) {
+    synchronized (m_dataStore.m_reserveLock) {
+      m_dataStore.m_reservedMask &= ~mask;
+    }
+  }
+
+  @Override
+  public Solenoid makeSolenoid(int channel) {
+    return new Solenoid(m_dataStore.m_module, PneumaticsModuleType.REVPH, channel);
+  }
+
+  @Override
+  public DoubleSolenoid makeDoubleSolenoid(int forwardChannel, int reverseChannel) {
+    return new DoubleSolenoid(
+        m_dataStore.m_module, PneumaticsModuleType.REVPH, forwardChannel, reverseChannel);
+  }
+
+  @Override
+  public Compressor makeCompressor() {
+    return new Compressor(m_dataStore.m_module, PneumaticsModuleType.REVPH);
+  }
+
+  @Override
+  public boolean reserveCompressor() {
+    synchronized (m_dataStore.m_reserveLock) {
+      if (m_dataStore.m_compressorReserved) {
+        return false;
+      }
+      m_dataStore.m_compressorReserved = true;
+      return true;
+    }
+  }
+
+  @Override
+  public void unreserveCompressor() {
+    synchronized (m_dataStore.m_reserveLock) {
+      m_dataStore.m_compressorReserved = false;
+    }
+  }
+
+  @Override
+  public int getSolenoidDisabledList() {
+    return REVPHJNI.getSolenoidDisabledList(m_handle);
+  }
+
+  /**
+   * Disables the compressor. The compressor will not turn on until {@link
+   * #enableCompressorDigital()}, {@link #enableCompressorAnalog(double, double)}, or {@link
+   * #enableCompressorHybrid(double, double)} are called.
+   */
+  @Override
+  public void disableCompressor() {
+    REVPHJNI.setClosedLoopControlDisabled(m_handle);
+  }
+
+  @Override
+  public void enableCompressorDigital() {
+    REVPHJNI.setClosedLoopControlDigital(m_handle);
+  }
+
+  /**
+   * Enables the compressor in analog mode. This mode uses an analog pressure sensor connected to
+   * analog channel 0 to cycle the compressor. The compressor will turn on when the pressure drops
+   * below {@code minPressure} and will turn off when the pressure reaches {@code maxPressure}.
+   *
+   * @param minPressure The minimum pressure in PSI. The compressor will turn on when the pressure
+   *     drops below this value. Range 0-120 PSI.
+   * @param maxPressure The maximum pressure in PSI. The compressor will turn off when the pressure
+   *     reaches this value. Range 0-120 PSI. Must be larger then minPressure.
+   */
+  @Override
+  public void enableCompressorAnalog(double minPressure, double maxPressure) {
+    if (minPressure >= maxPressure) {
+      throw new IllegalArgumentException("maxPressure must be greater than minPressure");
+    }
+    if (minPressure < 0 || minPressure > 120) {
+      throw new IllegalArgumentException(
+          "minPressure must be between 0 and 120 PSI, got " + minPressure);
+    }
+    if (maxPressure < 0 || maxPressure > 120) {
+      throw new IllegalArgumentException(
+          "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
+    }
+
+    // Send the voltage as it would be if the 5V rail was at exactly 5V.
+    // The firmware will compensate for the real 5V rail voltage, which
+    // can fluctuate somewhat over time.
+    double minAnalogVoltage = psiToVolts(minPressure, 5);
+    double maxAnalogVoltage = psiToVolts(maxPressure, 5);
+    REVPHJNI.setClosedLoopControlAnalog(m_handle, minAnalogVoltage, maxAnalogVoltage);
+  }
+
+  /**
+   * Enables the compressor in hybrid mode. This mode uses both a digital pressure switch and an
+   * analog pressure sensor connected to analog channel 0 to cycle the compressor.
+   *
+   * <p>The compressor will turn on when <i>both</i>:
+   *
+   * <ul>
+   *   <li>The digital pressure switch indicates the system is not full AND
+   *   <li>The analog pressure sensor indicates that the pressure in the system is below the
+   *       specified minimum pressure.
+   * </ul>
+   *
+   * <p>The compressor will turn off when <i>either</i>:
+   *
+   * <ul>
+   *   <li>The digital pressure switch is disconnected or indicates that the system is full OR
+   *   <li>The pressure detected by the analog sensor is greater than the specified maximum
+   *       pressure.
+   * </ul>
+   *
+   * @param minPressure The minimum pressure in PSI. The compressor will turn on when the pressure
+   *     drops below this value and the pressure switch indicates that the system is not full. Range
+   *     0-120 PSI.
+   * @param maxPressure The maximum pressure in PSI. The compressor will turn off when the pressure
+   *     reaches this value or the pressure switch is disconnected or indicates that the system is
+   *     full. Range 0-120 PSI. Must be larger then minPressure.
+   */
+  @Override
+  public void enableCompressorHybrid(double minPressure, double maxPressure) {
+    if (minPressure >= maxPressure) {
+      throw new IllegalArgumentException("maxPressure must be greater than minPressure");
+    }
+    if (minPressure < 0 || minPressure > 120) {
+      throw new IllegalArgumentException(
+          "minPressure must be between 0 and 120 PSI, got " + minPressure);
+    }
+    if (maxPressure < 0 || maxPressure > 120) {
+      throw new IllegalArgumentException(
+          "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
+    }
+
+    // Send the voltage as it would be if the 5V rail was at exactly 5V.
+    // The firmware will compensate for the real 5V rail voltage, which
+    // can fluctuate somewhat over time.
+    double minAnalogVoltage = psiToVolts(minPressure, 5);
+    double maxAnalogVoltage = psiToVolts(maxPressure, 5);
+    REVPHJNI.setClosedLoopControlHybrid(m_handle, minAnalogVoltage, maxAnalogVoltage);
+  }
+
+  /**
+   * Returns the raw voltage of the specified analog input channel.
+   *
+   * @param channel The analog input channel to read voltage from.
+   * @return The voltage of the specified analog input channel.
+   */
+  @Override
+  public double getAnalogVoltage(int channel) {
+    return REVPHJNI.getAnalogVoltage(m_handle, channel);
+  }
+
+  /**
+   * Returns the pressure read by an analog pressure sensor on the specified analog input channel.
+   *
+   * @param channel The analog input channel to read pressure from.
+   * @return The pressure read by an analog pressure sensor on the specified analog input channel.
+   */
+  @Override
+  public double getPressure(int channel) {
+    double sensorVoltage = REVPHJNI.getAnalogVoltage(m_handle, channel);
+    double supplyVoltage = REVPHJNI.get5VVoltage(m_handle);
+    return voltsToPsi(sensorVoltage, supplyVoltage);
+  }
+
+  /** Clears the sticky faults. */
+  public void clearStickyFaults() {
+    REVPHJNI.clearStickyFaults(m_handle);
+  }
+
+  /**
+   * Returns the hardware and firmware versions of this device.
+   *
+   * @return The hardware and firmware versions.
+   */
+  public REVPHVersion getVersion() {
+    return REVPHJNI.getVersion(m_handle);
+  }
+
+  /**
+   * Returns the faults currently active on this device.
+   *
+   * @return The faults.
+   */
+  public REVPHFaults getFaults() {
+    return REVPHJNI.getFaults(m_handle);
+  }
+
+  /**
+   * Returns the sticky faults currently active on this device.
+   *
+   * @return The sticky faults.
+   */
+  public REVPHStickyFaults getStickyFaults() {
+    return REVPHJNI.getStickyFaults(m_handle);
+  }
+
+  /**
+   * Returns the current input voltage for this device.
+   *
+   * @return The input voltage.
+   */
+  public double getInputVoltage() {
+    return REVPHJNI.getInputVoltage(m_handle);
+  }
+
+  /**
+   * Returns the current voltage of the regulated 5v supply.
+   *
+   * @return The current voltage of the 5v supply.
+   */
+  public double get5VRegulatedVoltage() {
+    return REVPHJNI.get5VVoltage(m_handle);
+  }
+
+  /**
+   * Returns the total current (in amps) drawn by all solenoids.
+   *
+   * @return Total current drawn by all solenoids in amps.
+   */
+  public double getSolenoidsTotalCurrent() {
+    return REVPHJNI.getSolenoidCurrent(m_handle);
+  }
+
+  /**
+   * Returns the current voltage of the solenoid power supply.
+   *
+   * @return The current voltage of the solenoid power supply.
+   */
+  public double getSolenoidsVoltage() {
+    return REVPHJNI.getSolenoidVoltage(m_handle);
+  }
+
+  @Override
+  public void reportUsage(String device, String data) {
+    HAL.reportUsage("PH[" + m_dataStore.m_module + "]/" + device, data);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/pneumatic/PneumaticsBase.java

@@ -0,0 +1,257 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+/** Interface for pneumatics devices. */
+public interface PneumaticsBase extends AutoCloseable {
+  /**
+   * For internal use to get a module for a specific type.
+   *
+   * @param busId The bus ID
+   * @param module module number
+   * @param type module type
+   * @return module
+   */
+  static PneumaticsBase getForType(int busId, int module, PneumaticsModuleType type) {
+    return switch (type) {
+      case CTREPCM -> new PneumaticsControlModule(busId, module);
+      case REVPH -> new PneumaticHub(busId, module);
+    };
+  }
+
+  /**
+   * For internal use to get the default for a specific type.
+   *
+   * @param type module type
+   * @return module default
+   */
+  static int getDefaultForType(PneumaticsModuleType type) {
+    return switch (type) {
+      case CTREPCM -> SensorUtil.getDefaultCTREPCMModule();
+      case REVPH -> SensorUtil.getDefaultREVPHModule();
+    };
+  }
+
+  /**
+   * Sets solenoids on a pneumatics module.
+   *
+   * @param mask Bitmask indicating which solenoids to set. The LSB represents solenoid 0.
+   * @param values Bitmask indicating the desired states of the solenoids. The LSB represents
+   *     solenoid 0.
+   */
+  void setSolenoids(int mask, int values);
+
+  /**
+   * Gets a bitmask of solenoid values.
+   *
+   * @return Bitmask containing the state of the solenoids. The LSB represents solenoid 0.
+   */
+  int getSolenoids();
+
+  /**
+   * Get module number for this module.
+   *
+   * @return module number
+   */
+  int getModuleNumber();
+
+  /**
+   * Get a bitmask of disabled solenoids.
+   *
+   * @return Bitmask indicating disabled solenoids. The LSB represents solenoid 0.
+   */
+  int getSolenoidDisabledList();
+
+  /**
+   * Fire a single solenoid shot.
+   *
+   * @param index solenoid index
+   */
+  void fireOneShot(int index);
+
+  /**
+   * Set the duration for a single solenoid shot.
+   *
+   * @param index solenoid index
+   * @param durMs shot duration
+   */
+  void setOneShotDuration(int index, int durMs);
+
+  /**
+   * Returns whether the compressor is active or not.
+   *
+   * @return True if the compressor is on - otherwise false.
+   */
+  boolean getCompressor();
+
+  /**
+   * Returns the state of the pressure switch.
+   *
+   * @return True if pressure switch indicates that the system is not full, otherwise false.
+   */
+  boolean getPressureSwitch();
+
+  /**
+   * Returns the current drawn by the compressor in amps.
+   *
+   * @return The current drawn by the compressor.
+   */
+  double getCompressorCurrent();
+
+  /** Disables the compressor. */
+  void disableCompressor();
+
+  /**
+   * Enables the compressor in digital mode using the digital pressure switch. The compressor will
+   * turn on when the pressure switch indicates that the system is not full, and will turn off when
+   * the pressure switch indicates that the system is full.
+   */
+  void enableCompressorDigital();
+
+  /**
+   * If supported by the device, enables the compressor in analog mode. This mode uses an analog
+   * pressure sensor connected to analog channel 0 to cycle the compressor. The compressor will turn
+   * on when the pressure drops below {@code minPressure} and will turn off when the pressure
+   * reaches {@code maxPressure}. This mode is only supported by the REV PH with the REV Analog
+   * Pressure Sensor connected to analog channel 0.
+   *
+   * <p>On CTRE PCM, this will enable digital control.
+   *
+   * @param minPressure The minimum pressure in PSI. The compressor will turn on when the pressure
+   *     drops below this value.
+   * @param maxPressure The maximum pressure in PSI. The compressor will turn off when the pressure
+   *     reaches this value.
+   */
+  void enableCompressorAnalog(double minPressure, double maxPressure);
+
+  /**
+   * If supported by the device, enables the compressor in hybrid mode. This mode uses both a
+   * digital pressure switch and an analog pressure sensor connected to analog channel 0 to cycle
+   * the compressor. This mode is only supported by the REV PH with the REV Analog Pressure Sensor
+   * connected to analog channel 0.
+   *
+   * <p>The compressor will turn on when <i>both</i>:
+   *
+   * <ul>
+   *   <li>The digital pressure switch indicates the system is not full AND
+   *   <li>The analog pressure sensor indicates that the pressure in the system is below the
+   *       specified minimum pressure.
+   * </ul>
+   *
+   * <p>The compressor will turn off when <i>either</i>:
+   *
+   * <ul>
+   *   <li>The digital pressure switch is disconnected or indicates that the system is full OR
+   *   <li>The pressure detected by the analog sensor is greater than the specified maximum
+   *       pressure.
+   * </ul>
+   *
+   * <p>On CTRE PCM, this will enable digital control.
+   *
+   * @param minPressure The minimum pressure in PSI. The compressor will turn on when the pressure
+   *     drops below this value and the pressure switch indicates that the system is not full.
+   * @param maxPressure The maximum pressure in PSI. The compressor will turn off when the pressure
+   *     reaches this value or the pressure switch is disconnected or indicates that the system is
+   *     full.
+   */
+  void enableCompressorHybrid(double minPressure, double maxPressure);
+
+  /**
+   * If supported by the device, returns the raw voltage of the specified analog input channel.
+   *
+   * <p>This function is only supported by the REV PH. On CTRE PCM, this will return 0.
+   *
+   * @param channel The analog input channel to read voltage from.
+   * @return The voltage of the specified analog input channel.
+   */
+  double getAnalogVoltage(int channel);
+
+  /**
+   * If supported by the device, returns the pressure (in PSI) read by an analog pressure sensor on
+   * the specified analog input channel.
+   *
+   * <p>This function is only supported by the REV PH. On CTRE PCM, this will return 0.
+   *
+   * @param channel The analog input channel to read pressure from.
+   * @return The pressure (in PSI) read by an analog pressure sensor on the specified analog input
+   *     channel.
+   */
+  double getPressure(int channel);
+
+  /**
+   * Returns the active compressor configuration.
+   *
+   * @return The active compressor configuration.
+   */
+  CompressorConfigType getCompressorConfigType();
+
+  /**
+   * Check if a solenoid channel is valid.
+   *
+   * @param channel Channel to check
+   * @return True if channel exists
+   */
+  boolean checkSolenoidChannel(int channel);
+
+  /**
+   * Check to see if the solenoids marked in the bitmask can be reserved, and if so, reserve them.
+   *
+   * @param mask The bitmask of solenoids to reserve. The LSB represents solenoid 0.
+   * @return 0 if successful; mask of solenoids that couldn't be allocated otherwise
+   */
+  int checkAndReserveSolenoids(int mask);
+
+  /**
+   * Unreserve the solenoids marked in the bitmask.
+   *
+   * @param mask The bitmask of solenoids to unreserve. The LSB represents solenoid 0.
+   */
+  void unreserveSolenoids(int mask);
+
+  /**
+   * Reserve the compressor.
+   *
+   * @return true if successful; false if compressor already reserved
+   */
+  boolean reserveCompressor();
+
+  /** Unreserve the compressor. */
+  void unreserveCompressor();
+
+  @Override
+  void close();
+
+  /**
+   * Create a solenoid object for the specified channel.
+   *
+   * @param channel solenoid channel
+   * @return Solenoid object
+   */
+  Solenoid makeSolenoid(int channel);
+
+  /**
+   * Create a double solenoid object for the specified channels.
+   *
+   * @param forwardChannel solenoid channel for forward
+   * @param reverseChannel solenoid channel for reverse
+   * @return DoubleSolenoid object
+   */
+  DoubleSolenoid makeDoubleSolenoid(int forwardChannel, int reverseChannel);
+
+  /**
+   * Create a compressor object.
+   *
+   * @return Compressor object
+   */
+  Compressor makeCompressor();
+
+  /**
+   * Report usage.
+   *
+   * @param device device and channel as appropriate
+   * @param data arbitrary usage data
+   */
+  void reportUsage(String device, String data);
+}

wpilibj/src/main/java/org/wpilib/hardware/pneumatic/PneumaticsControlModule.java

@@ -0,0 +1,370 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.CTREPCMJNI;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.PortsJNI;
+import java.util.HashMap;
+import java.util.Map;
+
+/** Module class for controlling a Cross The Road Electronics Pneumatics Control Module. */
+public class PneumaticsControlModule implements PneumaticsBase {
+  private static class DataStore implements AutoCloseable {
+    public final int m_module;
+    public final int m_handle;
+    private final int m_busId;
+    private int m_refCount;
+    private int m_reservedMask;
+    private boolean m_compressorReserved;
+    private final Object m_reserveLock = new Object();
+
+    DataStore(int busId, int module) {
+      m_handle = CTREPCMJNI.initialize(busId, module);
+      m_module = module;
+      m_busId = busId;
+      m_handleMaps[busId].put(module, this);
+    }
+
+    @Override
+    public void close() {
+      CTREPCMJNI.free(m_handle);
+      m_handleMaps[m_busId].remove(m_module);
+    }
+
+    public void addRef() {
+      m_refCount++;
+    }
+
+    public void removeRef() {
+      m_refCount--;
+      if (m_refCount == 0) {
+        this.close();
+      }
+    }
+  }
+
+  @SuppressWarnings({"unchecked", "rawtypes"})
+  private static final Map<Integer, DataStore>[] m_handleMaps =
+      (Map<Integer, DataStore>[]) new Map[PortsJNI.getNumCanBuses()];
+
+  private static final Object m_handleLock = new Object();
+
+  private static DataStore getForModule(int busId, int module) {
+    synchronized (m_handleLock) {
+      Map<Integer, DataStore> handleMap = m_handleMaps[busId];
+      if (handleMap == null) {
+        handleMap = new HashMap<>();
+        m_handleMaps[busId] = handleMap;
+      }
+
+      DataStore pcm = handleMap.get(module);
+      if (pcm == null) {
+        pcm = new DataStore(busId, module);
+      }
+      pcm.addRef();
+      return pcm;
+    }
+  }
+
+  private static void freeModule(DataStore store) {
+    synchronized (m_handleLock) {
+      store.removeRef();
+    }
+  }
+
+  private final DataStore m_dataStore;
+  private final int m_handle;
+
+  /**
+   * Constructs a PneumaticsControlModule with the default ID (0).
+   *
+   * @param busId The bus ID
+   */
+  public PneumaticsControlModule(int busId) {
+    this(busId, SensorUtil.getDefaultCTREPCMModule());
+  }
+
+  /**
+   * Constructs a PneumaticsControlModule.
+   *
+   * @param busId The bus ID
+   * @param module module number to construct
+   */
+  public PneumaticsControlModule(int busId, int module) {
+    m_dataStore = getForModule(busId, module);
+    m_handle = m_dataStore.m_handle;
+  }
+
+  @Override
+  public void close() {
+    freeModule(m_dataStore);
+  }
+
+  @Override
+  public boolean getCompressor() {
+    return CTREPCMJNI.getCompressor(m_handle);
+  }
+
+  @Override
+  public boolean getPressureSwitch() {
+    return CTREPCMJNI.getPressureSwitch(m_handle);
+  }
+
+  @Override
+  public double getCompressorCurrent() {
+    return CTREPCMJNI.getCompressorCurrent(m_handle);
+  }
+
+  /**
+   * Return whether the compressor current is currently too high.
+   *
+   * @return True if the compressor current is too high, otherwise false.
+   * @see #getCompressorCurrentTooHighStickyFault()
+   */
+  public boolean getCompressorCurrentTooHighFault() {
+    return CTREPCMJNI.getCompressorCurrentTooHighFault(m_handle);
+  }
+
+  /**
+   * Returns whether the compressor current has been too high since sticky faults were last cleared.
+   * This fault is persistent and can be cleared by {@link #clearAllStickyFaults()}
+   *
+   * @return True if the compressor current has been too high since sticky faults were last cleared.
+   * @see #getCompressorCurrentTooHighFault()
+   */
+  public boolean getCompressorCurrentTooHighStickyFault() {
+    return CTREPCMJNI.getCompressorCurrentTooHighStickyFault(m_handle);
+  }
+
+  /**
+   * Returns whether the compressor is currently shorted.
+   *
+   * @return True if the compressor is currently shorted, otherwise false.
+   * @see #getCompressorShortedStickyFault()
+   */
+  public boolean getCompressorShortedFault() {
+    return CTREPCMJNI.getCompressorShortedFault(m_handle);
+  }
+
+  /**
+   * Returns whether the compressor has been shorted since sticky faults were last cleared. This
+   * fault is persistent and can be cleared by {@link #clearAllStickyFaults()}
+   *
+   * @return True if the compressor has been shorted since sticky faults were last cleared,
+   *     otherwise false.
+   * @see #getCompressorShortedFault()
+   */
+  public boolean getCompressorShortedStickyFault() {
+    return CTREPCMJNI.getCompressorShortedStickyFault(m_handle);
+  }
+
+  /**
+   * Returns whether the compressor is currently disconnected.
+   *
+   * @return True if compressor is currently disconnected, otherwise false.
+   * @see #getCompressorNotConnectedStickyFault()
+   */
+  public boolean getCompressorNotConnectedFault() {
+    return CTREPCMJNI.getCompressorNotConnectedFault(m_handle);
+  }
+
+  /**
+   * Returns whether the compressor has been disconnected since sticky faults were last cleared.
+   * This fault is persistent and can be cleared by {@link #clearAllStickyFaults()}
+   *
+   * @return True if the compressor has been disconnected since sticky faults were last cleared,
+   *     otherwise false.
+   * @see #getCompressorNotConnectedFault()
+   */
+  public boolean getCompressorNotConnectedStickyFault() {
+    return CTREPCMJNI.getCompressorNotConnectedStickyFault(m_handle);
+  }
+
+  @Override
+  public void setSolenoids(int mask, int values) {
+    CTREPCMJNI.setSolenoids(m_handle, mask, values);
+  }
+
+  @Override
+  public int getSolenoids() {
+    return CTREPCMJNI.getSolenoids(m_handle);
+  }
+
+  @Override
+  public int getModuleNumber() {
+    return m_dataStore.m_module;
+  }
+
+  @Override
+  public int getSolenoidDisabledList() {
+    return CTREPCMJNI.getSolenoidDisabledList(m_handle);
+  }
+
+  /**
+   * Returns whether the solenoid is currently reporting a voltage fault.
+   *
+   * @return True if solenoid is reporting a fault, otherwise false.
+   * @see #getSolenoidVoltageStickyFault()
+   */
+  public boolean getSolenoidVoltageFault() {
+    return CTREPCMJNI.getSolenoidVoltageFault(m_handle);
+  }
+
+  /**
+   * Returns whether the solenoid has reported a voltage fault since sticky faults were last
+   * cleared. This fault is persistent and can be cleared by ClearAllStickyFaults()
+   *
+   * @return True if solenoid is reporting a fault, otherwise false.
+   * @see #getSolenoidVoltageFault()
+   */
+  public boolean getSolenoidVoltageStickyFault() {
+    return CTREPCMJNI.getSolenoidVoltageStickyFault(m_handle);
+  }
+
+  /** Clears all sticky faults on this device. */
+  public void clearAllStickyFaults() {
+    CTREPCMJNI.clearAllStickyFaults(m_handle);
+  }
+
+  @Override
+  public void fireOneShot(int index) {
+    CTREPCMJNI.fireOneShot(m_handle, index);
+  }
+
+  @Override
+  public void setOneShotDuration(int index, int durMs) {
+    CTREPCMJNI.setOneShotDuration(m_handle, index, durMs);
+  }
+
+  @Override
+  public boolean checkSolenoidChannel(int channel) {
+    return CTREPCMJNI.checkSolenoidChannel(channel);
+  }
+
+  @Override
+  public int checkAndReserveSolenoids(int mask) {
+    synchronized (m_dataStore.m_reserveLock) {
+      if ((m_dataStore.m_reservedMask & mask) != 0) {
+        return m_dataStore.m_reservedMask & mask;
+      }
+      m_dataStore.m_reservedMask |= mask;
+      return 0;
+    }
+  }
+
+  @Override
+  public void unreserveSolenoids(int mask) {
+    synchronized (m_dataStore.m_reserveLock) {
+      m_dataStore.m_reservedMask &= ~mask;
+    }
+  }
+
+  @Override
+  public Solenoid makeSolenoid(int channel) {
+    return new Solenoid(m_dataStore.m_module, PneumaticsModuleType.CTREPCM, channel);
+  }
+
+  @Override
+  public DoubleSolenoid makeDoubleSolenoid(int forwardChannel, int reverseChannel) {
+    return new DoubleSolenoid(
+        m_dataStore.m_module, PneumaticsModuleType.CTREPCM, forwardChannel, reverseChannel);
+  }
+
+  @Override
+  public Compressor makeCompressor() {
+    return new Compressor(m_dataStore.m_module, PneumaticsModuleType.CTREPCM);
+  }
+
+  @Override
+  public boolean reserveCompressor() {
+    synchronized (m_dataStore.m_reserveLock) {
+      if (m_dataStore.m_compressorReserved) {
+        return false;
+      }
+      m_dataStore.m_compressorReserved = true;
+      return true;
+    }
+  }
+
+  @Override
+  public void unreserveCompressor() {
+    synchronized (m_dataStore.m_reserveLock) {
+      m_dataStore.m_compressorReserved = false;
+    }
+  }
+
+  /**
+   * Disables the compressor. The compressor will not turn on until {@link
+   * #enableCompressorDigital()} is called.
+   */
+  @Override
+  public void disableCompressor() {
+    CTREPCMJNI.setClosedLoopControl(m_handle, false);
+  }
+
+  @Override
+  public void enableCompressorDigital() {
+    CTREPCMJNI.setClosedLoopControl(m_handle, true);
+  }
+
+  /**
+   * Enables the compressor in digital mode. Analog mode is unsupported by the CTRE PCM.
+   *
+   * @param minPressure Unsupported.
+   * @param maxPressure Unsupported.
+   * @see #enableCompressorDigital()
+   */
+  @Override
+  public void enableCompressorAnalog(double minPressure, double maxPressure) {
+    CTREPCMJNI.setClosedLoopControl(m_handle, false);
+  }
+
+  /**
+   * Enables the compressor in digital mode. Hybrid mode is unsupported by the CTRE PCM.
+   *
+   * @param minPressure Unsupported.
+   * @param maxPressure Unsupported.
+   * @see #enableCompressorDigital()
+   */
+  @Override
+  public void enableCompressorHybrid(double minPressure, double maxPressure) {
+    CTREPCMJNI.setClosedLoopControl(m_handle, false);
+  }
+
+  @Override
+  public CompressorConfigType getCompressorConfigType() {
+    return CTREPCMJNI.getClosedLoopControl(m_handle)
+        ? CompressorConfigType.Digital
+        : CompressorConfigType.Disabled;
+  }
+
+  /**
+   * Unsupported by the CTRE PCM.
+   *
+   * @param channel Unsupported.
+   * @return 0
+   */
+  @Override
+  public double getAnalogVoltage(int channel) {
+    return 0;
+  }
+
+  /**
+   * Unsupported by the CTRE PCM.
+   *
+   * @param channel Unsupported.
+   * @return 0
+   */
+  @Override
+  public double getPressure(int channel) {
+    return 0;
+  }
+
+  @Override
+  public void reportUsage(String device, String data) {
+    HAL.reportUsage("PCM[" + m_dataStore.m_module + "]/" + device, data);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/pneumatic/PneumaticsModuleType.java

@@ -0,0 +1,13 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+/** Pneumatics module type. */
+public enum PneumaticsModuleType {
+  /** CTRE PCM. */
+  CTREPCM,
+  /** REV PH. */
+  REVPH
+}

wpilibj/src/main/java/org/wpilib/hardware/pneumatic/Solenoid.java

@@ -0,0 +1,154 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Solenoid class for running high voltage Digital Output on a pneumatics module.
+ *
+ * <p>The Solenoid class is typically used for pneumatic solenoids, but could be used for any device
+ * within the current spec of the module.
+ */
+public class Solenoid implements Sendable, AutoCloseable {
+  private final int m_mask; // The channel mask
+  private final int m_channel;
+  private PneumaticsBase m_module;
+
+  /**
+   * Constructs a solenoid for a default module and specified type.
+   *
+   * @param busId The bus ID
+   * @param moduleType The module type to use.
+   * @param channel The channel the solenoid is on.
+   */
+  public Solenoid(final int busId, final PneumaticsModuleType moduleType, final int channel) {
+    this(busId, PneumaticsBase.getDefaultForType(moduleType), moduleType, channel);
+  }
+
+  /**
+   * Constructs a solenoid for a specified module and type.
+   *
+   * @param busId The bus ID
+   * @param module The module ID to use.
+   * @param moduleType The module type to use.
+   * @param channel The channel the solenoid is on.
+   */
+  @SuppressWarnings("this-escape")
+  public Solenoid(
+      final int busId, final int module, final PneumaticsModuleType moduleType, final int channel) {
+    m_module = PneumaticsBase.getForType(busId, module, moduleType);
+
+    m_mask = 1 << channel;
+    m_channel = channel;
+
+    if (!m_module.checkSolenoidChannel(channel)) {
+      m_module.close();
+      throw new IllegalArgumentException("Channel " + channel + " out of range");
+    }
+
+    if (m_module.checkAndReserveSolenoids(m_mask) != 0) {
+      m_module.close();
+      throw new AllocationException("Solenoid already allocated");
+    }
+
+    m_module.reportUsage("Solenoid[" + channel + "]", "Solenoid");
+    SendableRegistry.add(this, "Solenoid", m_module.getModuleNumber(), channel);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    m_module.unreserveSolenoids(m_mask);
+    m_module.close();
+    m_module = null;
+  }
+
+  /**
+   * Set the value of a solenoid.
+   *
+   * @param on True will turn the solenoid output on. False will turn the solenoid output off.
+   */
+  public void set(boolean on) {
+    int value = on ? (0xFFFF & m_mask) : 0;
+    m_module.setSolenoids(m_mask, value);
+  }
+
+  /**
+   * Read the current value of the solenoid.
+   *
+   * @return True if the solenoid output is on or false if the solenoid output is off.
+   */
+  public boolean get() {
+    int currentAll = m_module.getSolenoids();
+    return (currentAll & m_mask) != 0;
+  }
+
+  /**
+   * Toggle the value of the solenoid.
+   *
+   * <p>If the solenoid is set to on, it'll be turned off. If the solenoid is set to off, it'll be
+   * turned on.
+   */
+  public void toggle() {
+    set(!get());
+  }
+
+  /**
+   * Get the channel this solenoid is connected to.
+   *
+   * @return The channel this solenoid is connected to.
+   */
+  public int getChannel() {
+    return m_channel;
+  }
+
+  /**
+   * Check if solenoid is DisabledListed. If a solenoid is shorted, it is added to the Disabled List
+   * and disabled until power cycle, or until faults are cleared.
+   *
+   * @return If solenoid is disabled due to short.
+   */
+  public boolean isDisabled() {
+    return (m_module.getSolenoidDisabledList() & m_mask) != 0;
+  }
+
+  /**
+   * Set the pulse duration in the pneumatics module. This is used in conjunction with the
+   * startPulse method to allow the pneumatics module to control the timing of a pulse.
+   *
+   * <p>On the PCM, the timing can be controlled in 0.01 second increments, with a maximum of 2.55
+   * seconds.
+   *
+   * <p>On the PH, the timing can be controlled in 0.001 second increments, with a maximum of 65.534
+   * seconds.
+   *
+   * @param duration The duration of the pulse in seconds.
+   * @see #startPulse()
+   */
+  public void setPulseDuration(double duration) {
+    long durationMS = (long) (duration * 1000);
+    m_module.setOneShotDuration(m_channel, (int) durationMS);
+  }
+
+  /**
+   * Trigger the pneumatics module to generate a pulse of the duration set in setPulseDuration.
+   *
+   * @see #setPulseDuration(double)
+   */
+  public void startPulse() {
+    m_module.fireOneShot(m_channel);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Solenoid");
+    builder.setActuator(true);
+    builder.addBooleanProperty("Value", this::get, this::set);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/power/PowerDistribution.java

@@ -0,0 +1,274 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.PowerDistributionFaults;
+import edu.wpi.first.hal.PowerDistributionJNI;
+import edu.wpi.first.hal.PowerDistributionStickyFaults;
+import edu.wpi.first.hal.PowerDistributionVersion;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Class for getting voltage, current, temperature, power and energy from the CTRE Power
+ * Distribution Panel (PDP) or REV Power Distribution Hub (PDH) over CAN.
+ */
+public class PowerDistribution implements Sendable, AutoCloseable {
+  private final int m_handle;
+  private final int m_module;
+
+  /** Default module number. */
+  public static final int kDefaultModule = PowerDistributionJNI.DEFAULT_MODULE;
+
+  /** Power distribution module type. */
+  public enum ModuleType {
+    /** CTRE (Cross The Road Electronics) Power Distribution Panel (PDP). */
+    kCTRE(PowerDistributionJNI.CTRE_TYPE),
+    /** REV Power Distribution Hub (PDH). */
+    kRev(PowerDistributionJNI.REV_TYPE);
+
+    /** ModuleType value. */
+    public final int value;
+
+    ModuleType(int value) {
+      this.value = value;
+    }
+  }
+
+  /**
+   * Constructs a PowerDistribution object.
+   *
+   * @param busId The bus ID
+   * @param module The CAN ID of the PDP/PDH.
+   * @param moduleType Module type (CTRE or REV).
+   */
+  @SuppressWarnings("this-escape")
+  public PowerDistribution(int busId, int module, ModuleType moduleType) {
+    m_handle = PowerDistributionJNI.initialize(busId, module, moduleType.value);
+    m_module = PowerDistributionJNI.getModuleNumber(m_handle);
+
+    if (moduleType == ModuleType.kCTRE) {
+      HAL.reportUsage("PDP", m_module, "");
+    } else {
+      HAL.reportUsage("PDH", m_module, "");
+    }
+    SendableRegistry.add(this, "PowerDistribution", m_module);
+  }
+
+  /**
+   * Constructs a PowerDistribution object.
+   *
+   * <p>Detects the connected PDP/PDH using the default CAN ID (0 for CTRE and 1 for REV).
+   *
+   * @param busId The bus ID
+   */
+  @SuppressWarnings("this-escape")
+  public PowerDistribution(int busId) {
+    m_handle =
+        PowerDistributionJNI.initialize(busId, kDefaultModule, PowerDistributionJNI.AUTOMATIC_TYPE);
+    m_module = PowerDistributionJNI.getModuleNumber(m_handle);
+
+    if (PowerDistributionJNI.getType(m_handle) == PowerDistributionJNI.CTRE_TYPE) {
+      HAL.reportUsage("PowerDistribution", m_module, "CTRE");
+    } else {
+      HAL.reportUsage("PowerDistribution", m_module, "Rev");
+    }
+
+    SendableRegistry.add(this, "PowerDistribution", m_module);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Gets the number of channels for this power distribution object.
+   *
+   * @return Number of output channels (16 for PDP, 24 for PDH).
+   */
+  public int getNumChannels() {
+    return PowerDistributionJNI.getNumChannels(m_handle);
+  }
+
+  /**
+   * Query the input voltage of the PDP/PDH.
+   *
+   * @return The voltage in volts
+   */
+  public double getVoltage() {
+    return PowerDistributionJNI.getVoltage(m_handle);
+  }
+
+  /**
+   * Query the temperature of the PDP.
+   *
+   * <p>Not supported on the Rev PDH and returns 0.
+   *
+   * @return The temperature in degrees Celsius
+   */
+  public double getTemperature() {
+    return PowerDistributionJNI.getTemperature(m_handle);
+  }
+
+  /**
+   * Query the current of a single channel of the PDP/PDH.
+   *
+   * @param channel The channel (0-15 for PDP, 0-23 for PDH) to query
+   * @return The current of the channel in Amperes
+   */
+  public double getCurrent(int channel) {
+    return PowerDistributionJNI.getChannelCurrent(m_handle, channel);
+  }
+
+  /**
+   * Query all currents of the PDP.
+   *
+   * @return The current of each channel in Amperes
+   */
+  public double[] getAllCurrents() {
+    double[] currents = new double[getNumChannels()];
+    PowerDistributionJNI.getAllCurrents(m_handle, currents);
+    return currents;
+  }
+
+  /**
+   * Query the current of all monitored channels.
+   *
+   * @return The current of all the channels in Amperes
+   */
+  public double getTotalCurrent() {
+    return PowerDistributionJNI.getTotalCurrent(m_handle);
+  }
+
+  /**
+   * Query the total power drawn from the monitored channels of the PDP.
+   *
+   * <p>Not supported on the Rev PDH and returns 0.
+   *
+   * @return the total power in Watts
+   */
+  public double getTotalPower() {
+    return PowerDistributionJNI.getTotalPower(m_handle);
+  }
+
+  /**
+   * Query the total energy drawn from the monitored channels of the PDP.
+   *
+   * <p>Not supported on the Rev PDH and returns 0.
+   *
+   * @return the total energy in Joules
+   */
+  public double getTotalEnergy() {
+    return PowerDistributionJNI.getTotalEnergy(m_handle);
+  }
+
+  /**
+   * Reset the total energy to 0 of the PDP.
+   *
+   * <p>Not supported on the Rev PDH and does nothing.
+   */
+  public void resetTotalEnergy() {
+    PowerDistributionJNI.resetTotalEnergy(m_handle);
+  }
+
+  /** Clear all PDP/PDH sticky faults. */
+  public void clearStickyFaults() {
+    PowerDistributionJNI.clearStickyFaults(m_handle);
+  }
+
+  /**
+   * Gets module number (CAN ID).
+   *
+   * @return The module number (CAN ID).
+   */
+  public int getModule() {
+    return m_module;
+  }
+
+  /**
+   * Gets the module type for this power distribution object.
+   *
+   * @return The module type
+   */
+  public ModuleType getType() {
+    int type = PowerDistributionJNI.getType(m_handle);
+    if (type == PowerDistributionJNI.REV_TYPE) {
+      return ModuleType.kRev;
+    } else {
+      return ModuleType.kCTRE;
+    }
+  }
+
+  /**
+   * Gets whether the PDH switchable channel is turned on or off. Returns false with the CTRE PDP.
+   *
+   * @return The output state of the PDH switchable channel
+   */
+  public boolean getSwitchableChannel() {
+    return PowerDistributionJNI.getSwitchableChannel(m_handle);
+  }
+
+  /**
+   * Sets the PDH switchable channel on or off. Does nothing with the CTRE PDP.
+   *
+   * @param enabled Whether to turn the PDH switchable channel on or off
+   */
+  public void setSwitchableChannel(boolean enabled) {
+    PowerDistributionJNI.setSwitchableChannel(m_handle, enabled);
+  }
+
+  /**
+   * Returns the power distribution version number.
+   *
+   * @return The power distribution version number.
+   */
+  public PowerDistributionVersion getVersion() {
+    return PowerDistributionJNI.getVersion(m_handle);
+  }
+
+  /**
+   * Returns the power distribution faults.
+   *
+   * <p>On a CTRE PDP, this will return an object with no faults active.
+   *
+   * @return The power distribution faults.
+   */
+  public PowerDistributionFaults getFaults() {
+    return PowerDistributionJNI.getFaults(m_handle);
+  }
+
+  /**
+   * Returns the power distribution sticky faults.
+   *
+   * <p>On a CTRE PDP, this will return an object with no faults active.
+   *
+   * @return The power distribution sticky faults.
+   */
+  public PowerDistributionStickyFaults getStickyFaults() {
+    return PowerDistributionJNI.getStickyFaults(m_handle);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("PowerDistribution");
+    int numChannels = getNumChannels();
+    for (int i = 0; i < numChannels; ++i) {
+      final int chan = i;
+      builder.addDoubleProperty(
+          "Chan" + i, () -> PowerDistributionJNI.getChannelCurrentNoError(m_handle, chan), null);
+    }
+    builder.addDoubleProperty(
+        "Voltage", () -> PowerDistributionJNI.getVoltageNoError(m_handle), null);
+    builder.addDoubleProperty(
+        "TotalCurrent", () -> PowerDistributionJNI.getTotalCurrent(m_handle), null);
+    builder.addBooleanProperty(
+        "SwitchableChannel",
+        () -> PowerDistributionJNI.getSwitchableChannelNoError(m_handle),
+        value -> PowerDistributionJNI.setSwitchableChannel(m_handle, value));
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/range/SharpIR.java

@@ -0,0 +1,146 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * SharpIR analog distance sensor class. These distance measuring sensors output an analog voltage
+ * corresponding to the detection distance.
+ *
+ * <p>Teams are advised that the case of these sensors are conductive and grounded, so they should
+ * not be mounted on a metallic surface on an FRC robot.
+ */
+@SuppressWarnings("MethodName")
+public class SharpIR implements Sendable, AutoCloseable {
+  private AnalogInput m_sensor;
+
+  private SimDevice m_simDevice;
+  private SimDouble m_simRange;
+
+  private final double m_A;
+  private final double m_B;
+  private final double m_min; // m
+  private final double m_max; // m
+
+  /**
+   * Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring distances from 20 cm to 150 cm.
+   *
+   * @param channel Analog input channel the sensor is connected to
+   * @return sensor object
+   */
+  public static SharpIR GP2Y0A02YK0F(int channel) {
+    return new SharpIR(channel, 62.28, -1.092, 0.2, 1.5);
+  }
+
+  /**
+   * Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring distances from 10 cm to 80 cm.
+   *
+   * @param channel Analog input channel the sensor is connected to
+   * @return sensor object
+   */
+  public static SharpIR GP2Y0A21YK0F(int channel) {
+    return new SharpIR(channel, 26.449, -1.226, 0.1, 0.8);
+  }
+
+  /**
+   * Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring distances from 4 cm to 30 cm.
+   *
+   * @param channel Analog input channel the sensor is connected to
+   * @return sensor object
+   */
+  public static SharpIR GP2Y0A41SK0F(int channel) {
+    return new SharpIR(channel, 12.354, -1.07, 0.04, 0.3);
+  }
+
+  /**
+   * Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring distances from 2 cm to 15 cm.
+   *
+   * @param channel Analog input channel the sensor is connected to
+   * @return sensor object
+   */
+  public static SharpIR GP2Y0A51SK0F(int channel) {
+    return new SharpIR(channel, 5.2819, -1.161, 0.02, 0.15);
+  }
+
+  /**
+   * Manually construct a SharpIR object. The distance is computed using this formula: A*v ^ B.
+   * Prefer to use one of the static factories to create this device instead.
+   *
+   * @param channel AnalogInput channel
+   * @param a Constant A
+   * @param b Constant B
+   * @param min Minimum distance to report in meters
+   * @param max Maximum distance to report in meters
+   */
+  @SuppressWarnings("this-escape")
+  public SharpIR(int channel, double a, double b, double min, double max) {
+    m_sensor = new AnalogInput(channel);
+
+    m_A = a;
+    m_B = b;
+    m_min = min;
+    m_max = max;
+
+    HAL.reportUsage("IO", channel, "SharpIR");
+    SendableRegistry.add(this, "SharpIR", channel);
+
+    m_simDevice = SimDevice.create("SharpIR", m_sensor.getChannel());
+    if (m_simDevice != null) {
+      m_simRange = m_simDevice.createDouble("Range (m)", Direction.kInput, 0.0);
+      m_sensor.setSimDevice(m_simDevice);
+    }
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    m_sensor.close();
+    m_sensor = null;
+
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+      m_simRange = null;
+    }
+  }
+
+  /**
+   * Get the analog input channel number.
+   *
+   * @return analog input channel
+   */
+  public int getChannel() {
+    return m_sensor.getChannel();
+  }
+
+  /**
+   * Get the range in meters from the distance sensor.
+   *
+   * @return range in meters of the target returned by the sensor
+   */
+  public double getRange() {
+    if (m_simRange != null) {
+      return Math.clamp(m_simRange.get(), m_min, m_max);
+    } else {
+      // Don't allow zero/negative values
+      var v = Math.max(m_sensor.getVoltage(), 0.00001);
+
+      return Math.clamp(m_A * Math.pow(v, m_B) * 1e-2, m_min, m_max);
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Ultrasonic");
+    builder.addDoubleProperty("Value", this::getRange, null);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/rotation/AnalogEncoder.java

@@ -0,0 +1,177 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/** Class for supporting continuous analog encoders, such as the US Digital MA3. */
+public class AnalogEncoder implements Sendable, AutoCloseable {
+  private final AnalogInput m_analogInput;
+  private boolean m_ownsAnalogInput;
+  private double m_fullRange;
+  private double m_expectedZero;
+  private double m_sensorMin;
+  private double m_sensorMax = 1.0;
+  private boolean m_isInverted;
+
+  private SimDevice m_simDevice;
+  private SimDouble m_simPosition;
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
+   *
+   * @param channel the analog input channel to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  public AnalogEncoder(int channel, double fullRange, double expectedZero) {
+    this(new AnalogInput(channel), fullRange, expectedZero);
+    m_ownsAnalogInput = true;
+  }
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * @param analogInput the analog input to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  @SuppressWarnings("this-escape")
+  public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZero) {
+    m_analogInput = analogInput;
+    init(fullRange, expectedZero);
+  }
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param channel the analog input channel to attach to
+   */
+  public AnalogEncoder(int channel) {
+    this(channel, 1.0, 0.0);
+  }
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param analogInput the analog input to attach to
+   */
+  @SuppressWarnings("this-escape")
+  public AnalogEncoder(AnalogInput analogInput) {
+    this(analogInput, 1.0, 0.0);
+  }
+
+  private void init(double fullRange, double expectedZero) {
+    m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel());
+
+    if (m_simDevice != null) {
+      m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0);
+    }
+
+    m_fullRange = fullRange;
+    m_expectedZero = expectedZero;
+
+    HAL.reportUsage("IO", m_analogInput.getChannel(), "AnalogEncoder");
+
+    SendableRegistry.add(this, "Analog Encoder", m_analogInput.getChannel());
+  }
+
+  private double mapSensorRange(double pos) {
+    // map sensor range
+    if (pos < m_sensorMin) {
+      pos = m_sensorMin;
+    }
+    if (pos > m_sensorMax) {
+      pos = m_sensorMax;
+    }
+    pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
+    return pos;
+  }
+
+  /**
+   * Get the encoder value.
+   *
+   * @return the encoder value scaled by the full range input
+   */
+  public double get() {
+    if (m_simPosition != null) {
+      return m_simPosition.get();
+    }
+
+    double analog = m_analogInput.getVoltage();
+    double pos = analog / RobotController.getVoltage3V3();
+
+    // Map sensor range if range isn't full
+    pos = mapSensorRange(pos);
+
+    // Compute full range and offset
+    pos = pos * m_fullRange - m_expectedZero;
+
+    // Map from 0 - Full Range
+    double result = MathUtil.inputModulus(pos, 0, m_fullRange);
+    // Invert if necessary
+    if (m_isInverted) {
+      return m_fullRange - result;
+    }
+    return result;
+  }
+
+  /**
+   * Set the encoder voltage percentage range. Analog sensors are not always fully stable at the end
+   * of their travel ranges. Shrinking this range down can help mitigate issues with that.
+   *
+   * @param min minimum voltage percentage (0-1 range)
+   * @param max maximum voltage percentage (0-1 range)
+   */
+  public void setVoltagePercentageRange(double min, double max) {
+    m_sensorMin = Math.clamp(min, 0.0, 1.0);
+    m_sensorMax = Math.clamp(max, 0.0, 1.0);
+  }
+
+  /**
+   * Set if this encoder is inverted.
+   *
+   * @param inverted true to invert the encoder, false otherwise
+   */
+  public void setInverted(boolean inverted) {
+    m_isInverted = inverted;
+  }
+
+  /**
+   * Get the channel number.
+   *
+   * @return The channel number.
+   */
+  public int getChannel() {
+    return m_analogInput.getChannel();
+  }
+
+  @Override
+  public void close() {
+    if (m_ownsAnalogInput) {
+      m_analogInput.close();
+    }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("AbsoluteEncoder");
+    builder.addDoubleProperty("Position", this::get, null);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/rotation/AnalogPotentiometer.java

@@ -0,0 +1,148 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Class for reading analog potentiometers. Analog potentiometers read in an analog voltage that
+ * corresponds to a position. The position is in whichever units you choose, by way of the scaling
+ * and offset constants passed to the constructor.
+ */
+public class AnalogPotentiometer implements Sendable, AutoCloseable {
+  private AnalogInput m_analogInput;
+  private boolean m_initAnalogInput;
+  private double m_fullRange;
+  private double m_offset;
+
+  /**
+   * AnalogPotentiometer constructor.
+   *
+   * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
+   * have a 270 degree potentiometer, and you want the output to be degrees with the halfway point
+   * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+   * point after scaling is 135 degrees. This will calculate the result from the fullRange times the
+   * fraction of the supply voltage, plus the offset.
+   *
+   * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board
+   *     and 4-7 are on the MXP port.
+   * @param fullRange The scaling to multiply the fraction by to get a meaningful unit.
+   * @param offset The offset to add to the scaled value for controlling the zero value
+   */
+  @SuppressWarnings("this-escape")
+  public AnalogPotentiometer(final int channel, double fullRange, double offset) {
+    this(new AnalogInput(channel), fullRange, offset);
+    m_initAnalogInput = true;
+    SendableRegistry.addChild(this, m_analogInput);
+  }
+
+  /**
+   * AnalogPotentiometer constructor.
+   *
+   * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
+   * have a 270 degree potentiometer, and you want the output to be degrees with the halfway point
+   * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+   * point after scaling is 135 degrees. This will calculate the result from the fullRange times the
+   * fraction of the supply voltage, plus the offset.
+   *
+   * @param input The {@link AnalogInput} this potentiometer is plugged into.
+   * @param fullRange The angular value (in desired units) representing the full 0-3.3V range of the
+   *     input.
+   * @param offset The angular value (in desired units) representing the angular output at 0V.
+   */
+  @SuppressWarnings("this-escape")
+  public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) {
+    SendableRegistry.add(this, "AnalogPotentiometer", input.getChannel());
+    m_analogInput = input;
+    m_initAnalogInput = false;
+
+    m_fullRange = fullRange;
+    m_offset = offset;
+  }
+
+  /**
+   * AnalogPotentiometer constructor.
+   *
+   * <p>Use the scale value so that the output produces meaningful values. I.E: you have a 270
+   * degree potentiometer, and you want the output to be degrees with the starting point as 0
+   * degrees. The scale value is 270.0(degrees).
+   *
+   * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board
+   *     and 4-7 are on the MXP port.
+   * @param scale The scaling to multiply the voltage by to get a meaningful unit.
+   */
+  public AnalogPotentiometer(final int channel, double scale) {
+    this(channel, scale, 0);
+  }
+
+  /**
+   * AnalogPotentiometer constructor.
+   *
+   * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
+   * have a 270 degree potentiometer, and you want the output to be degrees with the starting point
+   * as 0 degrees. The scale value is 270.0(degrees).
+   *
+   * @param input The {@link AnalogInput} this potentiometer is plugged into.
+   * @param scale The scaling to multiply the voltage by to get a meaningful unit.
+   */
+  public AnalogPotentiometer(final AnalogInput input, double scale) {
+    this(input, scale, 0);
+  }
+
+  /**
+   * AnalogPotentiometer constructor.
+   *
+   * <p>The potentiometer will return a value between 0 and 1.0.
+   *
+   * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board
+   *     and 4-7 are on the MXP port.
+   */
+  public AnalogPotentiometer(final int channel) {
+    this(channel, 1, 0);
+  }
+
+  /**
+   * AnalogPotentiometer constructor.
+   *
+   * <p>The potentiometer will return a value between 0 and 1.0.
+   *
+   * @param input The {@link AnalogInput} this potentiometer is plugged into.
+   */
+  public AnalogPotentiometer(final AnalogInput input) {
+    this(input, 1, 0);
+  }
+
+  /**
+   * Get the current reading of the potentiometer.
+   *
+   * @return The current position of the potentiometer.
+   */
+  public double get() {
+    if (m_analogInput == null) {
+      return m_offset;
+    }
+    return (m_analogInput.getVoltage() / RobotController.getVoltage3V3()) * m_fullRange + m_offset;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    if (m_analogInput != null) {
+      builder.setSmartDashboardType("Analog Input");
+      builder.addDoubleProperty("Value", this::get, null);
+    }
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    if (m_initAnalogInput) {
+      m_analogInput.close();
+      m_analogInput = null;
+      m_initAnalogInput = false;
+    }
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/rotation/DutyCycle.java

@@ -0,0 +1,92 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.DutyCycleJNI;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Class to read a duty cycle PWM input.
+ *
+ * <p>PWM input signals are specified with a frequency and a ratio of high to low in that frequency.
+ * There are 8 of these in the roboRIO, and they can be attached to any SmartIO Channel.
+ *
+ * <p>These can be combined as the input of an AnalogTrigger to a Counter in order to implement
+ * rollover checking.
+ */
+public class DutyCycle implements Sendable, AutoCloseable {
+  // Explicitly package private
+  final int m_handle;
+  private final int m_channel;
+
+  /**
+   * Constructs a DutyCycle input from a smartio channel.
+   *
+   * @param channel The channel to use.
+   */
+  @SuppressWarnings("this-escape")
+  public DutyCycle(int channel) {
+    m_handle = DutyCycleJNI.initialize(channel);
+
+    m_channel = channel;
+    HAL.reportUsage("IO", channel, "DutyCycle");
+    SendableRegistry.add(this, "Duty Cycle", channel);
+  }
+
+  /** Close the DutyCycle and free all resources. */
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    DutyCycleJNI.free(m_handle);
+  }
+
+  /**
+   * Get the frequency of the duty cycle signal.
+   *
+   * @return frequency in Hertz
+   */
+  public double getFrequency() {
+    return DutyCycleJNI.getFrequency(m_handle);
+  }
+
+  /**
+   * Get the output ratio of the duty cycle signal.
+   *
+   * <p>0 means always low, 1 means always high.
+   *
+   * @return output ratio between 0 and 1
+   */
+  public double getOutput() {
+    return DutyCycleJNI.getOutput(m_handle);
+  }
+
+  /**
+   * Get the raw high time of the duty cycle signal.
+   *
+   * @return high time of last pulse in nanoseconds
+   */
+  public int getHighTimeNanoseconds() {
+    return DutyCycleJNI.getHighTime(m_handle);
+  }
+
+  /**
+   * Get the channel of the source.
+   *
+   * @return the source channel
+   */
+  public int getSourceChannel() {
+    return m_channel;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Duty Cycle");
+    builder.addDoubleProperty("Frequency", this::getFrequency, null);
+    builder.addDoubleProperty("Output", this::getOutput, null);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/rotation/DutyCycleEncoder.java

@@ -0,0 +1,253 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the
+ * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder.
+ */
+public class DutyCycleEncoder implements Sendable, AutoCloseable {
+  private final DutyCycle m_dutyCycle;
+  private boolean m_ownsDutyCycle;
+  private double m_frequencyThreshold = 100;
+  private double m_fullRange;
+  private double m_expectedZero;
+  private double m_periodNanos;
+  private double m_sensorMin;
+  private double m_sensorMax = 1.0;
+  private boolean m_isInverted;
+
+  private SimDevice m_simDevice;
+  private SimDouble m_simPosition;
+  private SimBoolean m_simIsConnected;
+
+  /**
+   * Construct a new DutyCycleEncoder on a specific channel.
+   *
+   * @param channel the channel to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  @SuppressWarnings("this-escape")
+  public DutyCycleEncoder(int channel, double fullRange, double expectedZero) {
+    m_ownsDutyCycle = true;
+    m_dutyCycle = new DutyCycle(channel);
+    init(fullRange, expectedZero);
+  }
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  @SuppressWarnings("this-escape")
+  public DutyCycleEncoder(DutyCycle dutyCycle, double fullRange, double expectedZero) {
+    m_dutyCycle = dutyCycle;
+    init(fullRange, expectedZero);
+  }
+
+  /**
+   * Construct a new DutyCycleEncoder on a specific channel.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param channel the channel to attach to
+   */
+  @SuppressWarnings("this-escape")
+  public DutyCycleEncoder(int channel) {
+    this(channel, 1.0, 0.0);
+  }
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   */
+  @SuppressWarnings("this-escape")
+  public DutyCycleEncoder(DutyCycle dutyCycle) {
+    this(dutyCycle, 1.0, 0.0);
+  }
+
+  private void init(double fullRange, double expectedZero) {
+    m_simDevice = SimDevice.create("DutyCycle:DutyCycleEncoder", m_dutyCycle.getSourceChannel());
+
+    if (m_simDevice != null) {
+      m_simPosition = m_simDevice.createDouble("Position", SimDevice.Direction.kInput, 0.0);
+      m_simIsConnected = m_simDevice.createBoolean("Connected", SimDevice.Direction.kInput, true);
+    }
+
+    m_fullRange = fullRange;
+    m_expectedZero = expectedZero;
+
+    SendableRegistry.add(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel());
+  }
+
+  private double mapSensorRange(double pos) {
+    // map sensor range
+    if (pos < m_sensorMin) {
+      pos = m_sensorMin;
+    }
+    if (pos > m_sensorMax) {
+      pos = m_sensorMax;
+    }
+    pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
+    return pos;
+  }
+
+  /**
+   * Get the encoder value since the last reset.
+   *
+   * <p>This is reported in rotations since the last reset.
+   *
+   * @return the encoder value in rotations
+   */
+  public double get() {
+    if (m_simPosition != null) {
+      return m_simPosition.get();
+    }
+
+    double pos;
+    // Compute output percentage (0-1)
+    if (m_periodNanos == 0.0) {
+      pos = m_dutyCycle.getOutput();
+    } else {
+      int highTime = m_dutyCycle.getHighTimeNanoseconds();
+      pos = highTime / m_periodNanos;
+    }
+
+    // Map sensor range if range isn't full
+    pos = mapSensorRange(pos);
+
+    // Compute full range and offset
+    pos = pos * m_fullRange - m_expectedZero;
+
+    // Map from 0 - Full Range
+    double result = MathUtil.inputModulus(pos, 0, m_fullRange);
+    // Invert if necessary
+    if (m_isInverted) {
+      return m_fullRange - result;
+    }
+    return result;
+  }
+
+  /**
+   * Set the encoder duty cycle range. As the encoder needs to maintain a duty cycle, the duty cycle
+   * cannot go all the way to 0% or all the way to 100%. For example, an encoder with a 4096 us
+   * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty cycle of 4095 /
+   * 4096 us. Setting the range will result in an encoder duty cycle less than or equal to the
+   * minimum being output as 0 rotation, the duty cycle greater than or equal to the maximum being
+   * output as 1 rotation, and values in between linearly scaled from 0 to 1.
+   *
+   * @param min minimum duty cycle (0-1 range)
+   * @param max maximum duty cycle (0-1 range)
+   */
+  public void setDutyCycleRange(double min, double max) {
+    m_sensorMin = Math.clamp(min, 0.0, 1.0);
+    m_sensorMax = Math.clamp(max, 0.0, 1.0);
+  }
+
+  /**
+   * Get the frequency in Hz of the duty cycle signal from the encoder.
+   *
+   * @return duty cycle frequency in Hz
+   */
+  public double getFrequency() {
+    return m_dutyCycle.getFrequency();
+  }
+
+  /**
+   * Get if the sensor is connected
+   *
+   * <p>This uses the duty cycle frequency to determine if the sensor is connected. By default, a
+   * value of 100 Hz is used as the threshold, and this value can be changed with {@link
+   * #setConnectedFrequencyThreshold(int)}.
+   *
+   * @return true if the sensor is connected
+   */
+  public boolean isConnected() {
+    if (m_simIsConnected != null) {
+      return m_simIsConnected.get();
+    }
+    return getFrequency() > m_frequencyThreshold;
+  }
+
+  /**
+   * Change the frequency threshold for detecting connection used by {@link #isConnected()}.
+   *
+   * @param frequency the minimum frequency in Hz.
+   */
+  public void setConnectedFrequencyThreshold(double frequency) {
+    if (frequency < 0) {
+      frequency = 0;
+    }
+
+    m_frequencyThreshold = frequency;
+  }
+
+  /**
+   * Sets the assumed frequency of the connected device.
+   *
+   * <p>By default, the DutyCycle engine has to compute the frequency of the input signal. This can
+   * result in both delayed readings and jumpy readings. To solve this, you can pass the expected
+   * frequency of the sensor to this function. This will use that frequency to compute the DutyCycle
+   * percentage, rather than the computed frequency.
+   *
+   * @param frequency the assumed frequency of the sensor
+   */
+  public void setAssumedFrequency(double frequency) {
+    if (frequency == 0.0) {
+      m_periodNanos = 0.0;
+    } else {
+      m_periodNanos = 1000000000 / frequency;
+    }
+  }
+
+  /**
+   * Set if this encoder is inverted.
+   *
+   * @param inverted true to invert the encoder, false otherwise
+   */
+  public void setInverted(boolean inverted) {
+    m_isInverted = inverted;
+  }
+
+  @Override
+  public void close() {
+    if (m_ownsDutyCycle) {
+      m_dutyCycle.close();
+    }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+    }
+  }
+
+  /**
+   * Get the channel of the source.
+   *
+   * @return the source channel
+   */
+  public int getSourceChannel() {
+    return m_dutyCycle.getSourceChannel();
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("AbsoluteEncoder");
+    builder.addDoubleProperty("Position", this::get, null);
+    builder.addBooleanProperty("Is Connected", this::isConnected, null);
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/rotation/Encoder.java

@@ -0,0 +1,357 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.hal.EncoderJNI;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Class to read quadrature encoders.
+ *
+ * <p>Quadrature encoders are devices that count shaft rotation and can sense direction. The output
+ * of the Encoder class is an integer that can count either up or down, and can go negative for
+ * reverse direction counting. When creating Encoders, a direction can be supplied that inverts the
+ * sense of the output to make code more readable if the encoder is mounted such that forward
+ * movement generates negative values. Quadrature encoders have two digital outputs, an A Channel
+ * and a B Channel, that are out of phase with each other for direction sensing.
+ *
+ * <p>All encoders will immediately start counting - reset() them if you need them to be zeroed
+ * before use.
+ */
+public class Encoder implements CounterBase, Sendable, AutoCloseable {
+  private final EncodingType m_encodingType;
+
+  int m_encoder; // the HAL encoder object
+
+  /**
+   * Common initialization code for Encoders. This code allocates resources for Encoders and is
+   * common to all constructors.
+   *
+   * <p>The encoder will start counting immediately.
+   *
+   * @param aChannel The a channel.
+   * @param bChannel The b channel.
+   * @param reverseDirection If true, counts down instead of up (this is all relative)
+   */
+  private void initEncoder(
+      int aChannel, int bChannel, boolean reverseDirection, final EncodingType type) {
+    m_encoder = EncoderJNI.initializeEncoder(aChannel, bChannel, reverseDirection, type.value);
+
+    String typeStr =
+        switch (type) {
+          case k1X -> "Encoder:1x";
+          case k2X -> "Encoder:2x";
+          case k4X -> "Encoder:4x";
+          default -> "Encoder";
+        };
+    HAL.reportUsage("IO[" + aChannel + "," + bChannel + "]", typeStr);
+
+    int fpgaIndex = getFPGAIndex();
+    SendableRegistry.add(this, "Encoder", fpgaIndex);
+  }
+
+  /**
+   * Encoder constructor. Construct a Encoder given a and b channels.
+   *
+   * <p>The encoder will start counting immediately.
+   *
+   * @param channelA The 'a' channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port
+   * @param channelB The 'b' channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port
+   * @param reverseDirection represents the orientation of the encoder and inverts the output values
+   *     if necessary so forward represents positive values.
+   */
+  public Encoder(final int channelA, final int channelB, boolean reverseDirection) {
+    this(channelA, channelB, reverseDirection, EncodingType.k4X);
+  }
+
+  /**
+   * Encoder constructor. Construct an Encoder given a and b channels.
+   *
+   * <p>The encoder will start counting immediately.
+   *
+   * @param channelA The a channel digital input channel.
+   * @param channelB The b channel digital input channel.
+   */
+  public Encoder(final int channelA, final int channelB) {
+    this(channelA, channelB, false);
+  }
+
+  /**
+   * Encoder constructor. Construct an Encoder given a and b channels.
+   *
+   * <p>The encoder will start counting immediately.
+   *
+   * @param channelA The a channel digital input channel.
+   * @param channelB The b channel digital input channel.
+   * @param reverseDirection represents the orientation of the encoder and inverts the output values
+   *     if necessary so forward represents positive values.
+   * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+   *     selected, then an encoder FPGA object is used and the returned counts will be 4x the
+   *     encoder spec'd value since all rising and falling edges are counted. If 1X or 2X are
+   *     selected, then a counter object will be used and the returned value will either exactly
+   *     match the spec'd count or be double (2x) the spec'd count.
+   */
+  @SuppressWarnings("this-escape")
+  public Encoder(
+      final int channelA,
+      final int channelB,
+      boolean reverseDirection,
+      final EncodingType encodingType) {
+    requireNonNullParam(encodingType, "encodingType", "Encoder");
+
+    m_encodingType = encodingType;
+    // SendableRegistry.addChild(this, m_aSource);
+    // SendableRegistry.addChild(this, m_bSource);
+    initEncoder(channelA, channelB, reverseDirection, encodingType);
+  }
+
+  /**
+   * Get the FPGA index of the encoder.
+   *
+   * @return The Encoder's FPGA index.
+   */
+  public int getFPGAIndex() {
+    return EncoderJNI.getEncoderFPGAIndex(m_encoder);
+  }
+
+  /**
+   * Used to divide raw edge counts down to spec'd counts.
+   *
+   * @return The encoding scale factor 1x, 2x, or 4x, per the requested encoding type.
+   */
+  public int getEncodingScale() {
+    return EncoderJNI.getEncoderEncodingScale(m_encoder);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    // if (m_aSource != null && m_allocatedA) {
+    //   m_aSource.close();
+    //   m_allocatedA = false;
+    // }
+    // if (m_bSource != null && m_allocatedB) {
+    //   m_bSource.close();
+    //   m_allocatedB = false;
+    // }
+    // if (m_indexSource != null && m_allocatedI) {
+    //   m_indexSource.close();
+    //   m_allocatedI = false;
+    // }
+
+    // m_aSource = null;
+    // m_bSource = null;
+    // m_indexSource = null;
+    EncoderJNI.freeEncoder(m_encoder);
+    m_encoder = 0;
+  }
+
+  /**
+   * Gets the raw value from the encoder. The raw value is the actual count unscaled by the 1x, 2x,
+   * or 4x scale factor.
+   *
+   * @return Current raw count from the encoder
+   */
+  public int getRaw() {
+    return EncoderJNI.getEncoderRaw(m_encoder);
+  }
+
+  /**
+   * Gets the current count. Returns the current count on the Encoder. This method compensates for
+   * the decoding type.
+   *
+   * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale factor.
+   */
+  @Override
+  public int get() {
+    return EncoderJNI.getEncoder(m_encoder);
+  }
+
+  /** Reset the Encoder distance to zero. Resets the current count to zero on the encoder. */
+  @Override
+  public void reset() {
+    EncoderJNI.resetEncoder(m_encoder);
+  }
+
+  /**
+   * Returns the period of the most recent pulse. Returns the period of the most recent Encoder
+   * pulse in seconds. This method compensates for the decoding type.
+   *
+   * <p><b>Warning:</b> This returns unscaled periods. Use getRate() for rates that are scaled using
+   * the value from setDistancePerPulse().
+   *
+   * @return Period in seconds of the most recent pulse.
+   * @deprecated Use getRate() in favor of this method.
+   */
+  @Override
+  @Deprecated
+  public double getPeriod() {
+    return EncoderJNI.getEncoderPeriod(m_encoder);
+  }
+
+  /**
+   * Sets the maximum period for stopped detection. Sets the value that represents the maximum
+   * period of the Encoder before it will assume that the attached device is stopped. This timeout
+   * allows users to determine if the wheels or other shaft has stopped rotating. This method
+   * compensates for the decoding type.
+   *
+   * @param maxPeriod The maximum time between rising and falling edges before the FPGA will report
+   *     the device stopped. This is expressed in seconds.
+   * @deprecated Use setMinRate() in favor of this method. This takes unscaled periods and
+   *     setMinRate() scales using value from setDistancePerPulse().
+   */
+  @Override
+  @Deprecated
+  public void setMaxPeriod(double maxPeriod) {
+    EncoderJNI.setEncoderMaxPeriod(m_encoder, maxPeriod);
+  }
+
+  /**
+   * Determine if the encoder is stopped. Using the MaxPeriod value, a boolean is returned that is
+   * true if the encoder is considered stopped and false if it is still moving. A stopped encoder is
+   * one where the most recent pulse width exceeds the MaxPeriod.
+   *
+   * @return True if the encoder is considered stopped.
+   */
+  @Override
+  public boolean getStopped() {
+    return EncoderJNI.getEncoderStopped(m_encoder);
+  }
+
+  /**
+   * The last direction the encoder value changed.
+   *
+   * @return The last direction the encoder value changed.
+   */
+  @Override
+  public boolean getDirection() {
+    return EncoderJNI.getEncoderDirection(m_encoder);
+  }
+
+  /**
+   * Get the distance the robot has driven since the last reset as scaled by the value from {@link
+   * #setDistancePerPulse(double)}.
+   *
+   * @return The distance driven since the last reset
+   */
+  public double getDistance() {
+    return EncoderJNI.getEncoderDistance(m_encoder);
+  }
+
+  /**
+   * Get the current rate of the encoder. Units are distance per second as scaled by the value from
+   * setDistancePerPulse().
+   *
+   * @return The current rate of the encoder.
+   */
+  public double getRate() {
+    return EncoderJNI.getEncoderRate(m_encoder);
+  }
+
+  /**
+   * Set the minimum rate of the device before the hardware reports it stopped.
+   *
+   * @param minRate The minimum rate. The units are in distance per second as scaled by the value
+   *     from setDistancePerPulse().
+   */
+  public void setMinRate(double minRate) {
+    EncoderJNI.setEncoderMinRate(m_encoder, minRate);
+  }
+
+  /**
+   * Set the distance per pulse for this encoder. This sets the multiplier used to determine the
+   * distance driven based on the count value from the encoder. Do not include the decoding type in
+   * this scale. The library already compensates for the decoding type. Set this value based on the
+   * encoder's rated Pulses per Revolution and factor in gearing reductions following the encoder
+   * shaft. This distance can be in any units you like, linear or angular.
+   *
+   * @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
+   */
+  public void setDistancePerPulse(double distancePerPulse) {
+    EncoderJNI.setEncoderDistancePerPulse(m_encoder, distancePerPulse);
+  }
+
+  /**
+   * Get the distance per pulse for this encoder.
+   *
+   * @return The scale factor that will be used to convert pulses to useful units.
+   */
+  public double getDistancePerPulse() {
+    return EncoderJNI.getEncoderDistancePerPulse(m_encoder);
+  }
+
+  /**
+   * Set the direction sensing for this encoder. This sets the direction sensing on the encoder so
+   * that it could count in the correct software direction regardless of the mounting.
+   *
+   * @param reverseDirection true if the encoder direction should be reversed
+   */
+  public void setReverseDirection(boolean reverseDirection) {
+    EncoderJNI.setEncoderReverseDirection(m_encoder, reverseDirection);
+  }
+
+  /**
+   * Set the Samples to Average which specifies the number of samples of the timer to average when
+   * calculating the period. Perform averaging to account for mechanical imperfections or as
+   * oversampling to increase resolution.
+   *
+   * @param samplesToAverage The number of samples to average from 1 to 127.
+   */
+  public void setSamplesToAverage(int samplesToAverage) {
+    EncoderJNI.setEncoderSamplesToAverage(m_encoder, samplesToAverage);
+  }
+
+  /**
+   * Get the Samples to Average which specifies the number of samples of the timer to average when
+   * calculating the period. Perform averaging to account for mechanical imperfections or as
+   * oversampling to increase resolution.
+   *
+   * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
+   */
+  public int getSamplesToAverage() {
+    return EncoderJNI.getEncoderSamplesToAverage(m_encoder);
+  }
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  public void setSimDevice(SimDevice device) {
+    EncoderJNI.setEncoderSimDevice(m_encoder, device.getNativeHandle());
+  }
+
+  /**
+   * Gets the decoding scale factor for scaling raw values to full counts.
+   *
+   * @return decoding scale factor
+   */
+  public double getDecodingScaleFactor() {
+    return switch (m_encodingType) {
+      case k1X -> 1.0;
+      case k2X -> 0.5;
+      case k4X -> 0.25;
+    };
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    if (EncoderJNI.getEncoderEncodingType(m_encoder) == EncodingType.k4X.value) {
+      builder.setSmartDashboardType("Quadrature Encoder");
+    } else {
+      builder.setSmartDashboardType("Encoder");
+    }
+
+    builder.addDoubleProperty("Speed", this::getRate, null);
+    builder.addDoubleProperty("Distance", this::getDistance, null);
+    builder.addDoubleProperty("Distance per Tick", this::getDistancePerPulse, null);
+  }
+}

wpilibj/src/main/java/org/wpilib/internal/DriverStationModeThread.java

@@ -0,0 +1,110 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.internal;
+
+import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/** For internal use only. */
+public class DriverStationModeThread implements AutoCloseable {
+  private final AtomicBoolean m_keepAlive = new AtomicBoolean();
+  private final Thread m_thread;
+
+  private boolean m_userInDisabled;
+  private boolean m_userInAutonomous;
+  private boolean m_userInTeleop;
+  private boolean m_userInTest;
+
+  /** Internal use only. */
+  public DriverStationModeThread() {
+    m_keepAlive.set(true);
+    m_thread = new Thread(this::run, "DriverStationMode");
+    m_thread.start();
+  }
+
+  private void run() {
+    int handle = WPIUtilJNI.createEvent(false, false);
+    DriverStationJNI.provideNewDataEventHandle(handle);
+
+    while (m_keepAlive.get()) {
+      try {
+        WPIUtilJNI.waitForObjectTimeout(handle, 0.1);
+      } catch (InterruptedException e) {
+        DriverStationJNI.removeNewDataEventHandle(handle);
+        WPIUtilJNI.destroyEvent(handle);
+        Thread.currentThread().interrupt();
+        return;
+      }
+      DriverStation.refreshData();
+      if (m_userInDisabled) {
+        DriverStationJNI.observeUserProgramDisabled();
+      }
+      if (m_userInAutonomous) {
+        DriverStationJNI.observeUserProgramAutonomous();
+      }
+      if (m_userInTeleop) {
+        DriverStationJNI.observeUserProgramTeleop();
+      }
+      if (m_userInTest) {
+        DriverStationJNI.observeUserProgramTest();
+      }
+    }
+
+    DriverStationJNI.removeNewDataEventHandle(handle);
+    WPIUtilJNI.destroyEvent(handle);
+  }
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+   * purposes only.
+   *
+   * @param entering If true, starting disabled code; if false, leaving disabled code
+   */
+  public void inDisabled(boolean entering) {
+    m_userInDisabled = entering;
+  }
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+   * purposes only.
+   *
+   * @param entering If true, starting autonomous code; if false, leaving autonomous code
+   */
+  public void inAutonomous(boolean entering) {
+    m_userInAutonomous = entering;
+  }
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+   * purposes only.
+   *
+   * @param entering If true, starting teleop code; if false, leaving teleop code
+   */
+  public void inTeleop(boolean entering) {
+    m_userInTeleop = entering;
+  }
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+   * purposes only.
+   *
+   * @param entering If true, starting test code; if false, leaving test code
+   */
+  public void inTest(boolean entering) {
+    m_userInTest = entering;
+  }
+
+  @Override
+  public void close() {
+    m_keepAlive.set(false);
+    try {
+      m_thread.join();
+    } catch (InterruptedException e) {
+      Thread.currentThread().interrupt();
+    }
+  }
+}

wpilibj/src/main/java/org/wpilib/opmode/IterativeRobotBase.java

@@ -0,0 +1,377 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * IterativeRobotBase implements a specific type of robot program framework, extending the RobotBase
+ * class.
+ *
+ * <p>The IterativeRobotBase class does not implement startCompetition(), so it should not be used
+ * by teams directly.
+ *
+ * <p>This class provides the following functions which are called by the main loop,
+ * startCompetition(), at the appropriate times:
+ *
+ * <p>driverStationConnected() -- provide for initialization the first time the DS is connected
+ *
+ * <p>init() functions -- each of the following functions is called once when the appropriate mode
+ * is entered:
+ *
+ * <ul>
+ *   <li>disabledInit() -- called each and every time disabled is entered from another mode
+ *   <li>autonomousInit() -- called each and every time autonomous is entered from another mode
+ *   <li>teleopInit() -- called each and every time teleop is entered from another mode
+ *   <li>testInit() -- called each and every time test is entered from another mode
+ * </ul>
+ *
+ * <p>periodic() functions -- each of these functions is called on an interval:
+ *
+ * <ul>
+ *   <li>robotPeriodic()
+ *   <li>disabledPeriodic()
+ *   <li>autonomousPeriodic()
+ *   <li>teleopPeriodic()
+ *   <li>testPeriodic()
+ * </ul>
+ *
+ * <p>exit() functions -- each of the following functions is called once when the appropriate mode
+ * is exited:
+ *
+ * <ul>
+ *   <li>disabledExit() -- called each and every time disabled is exited
+ *   <li>autonomousExit() -- called each and every time autonomous is exited
+ *   <li>teleopExit() -- called each and every time teleop is exited
+ *   <li>testExit() -- called each and every time test is exited
+ * </ul>
+ */
+public abstract class IterativeRobotBase extends RobotBase {
+  private enum Mode {
+    kNone,
+    kDisabled,
+    kAutonomous,
+    kTeleop,
+    kTest
+  }
+
+  private final DSControlWord m_word = new DSControlWord();
+  private Mode m_lastMode = Mode.kNone;
+  private final double m_period;
+  private final Watchdog m_watchdog;
+  private boolean m_ntFlushEnabled = true;
+  private boolean m_calledDsConnected;
+
+  /**
+   * Constructor for IterativeRobotBase.
+   *
+   * @param period Period in seconds.
+   */
+  protected IterativeRobotBase(double period) {
+    m_period = period;
+    m_watchdog = new Watchdog(period, this::printLoopOverrunMessage);
+  }
+
+  /** Provide an alternate "main loop" via startCompetition(). */
+  @Override
+  public abstract void startCompetition();
+
+  /* ----------- Overridable initialization code ----------------- */
+
+  /**
+   * Code that needs to know the DS state should go here.
+   *
+   * <p>Users should override this method for initialization that needs to occur after the DS is
+   * connected, such as needing the alliance information.
+   */
+  public void driverStationConnected() {}
+
+  /**
+   * Robot-wide simulation initialization code should go here.
+   *
+   * <p>Users should override this method for default Robot-wide simulation related initialization
+   * which will be called when the robot is first started. It will be called exactly one time after
+   * the robot class constructor is called only when the robot is in simulation.
+   */
+  public void simulationInit() {}
+
+  /**
+   * Initialization code for disabled mode should go here.
+   *
+   * <p>Users should override this method for initialization code which will be called each time the
+   * robot enters disabled mode.
+   */
+  public void disabledInit() {}
+
+  /**
+   * Initialization code for autonomous mode should go here.
+   *
+   * <p>Users should override this method for initialization code which will be called each time the
+   * robot enters autonomous mode.
+   */
+  public void autonomousInit() {}
+
+  /**
+   * Initialization code for teleop mode should go here.
+   *
+   * <p>Users should override this method for initialization code which will be called each time the
+   * robot enters teleop mode.
+   */
+  public void teleopInit() {}
+
+  /**
+   * Initialization code for test mode should go here.
+   *
+   * <p>Users should override this method for initialization code which will be called each time the
+   * robot enters test mode.
+   */
+  public void testInit() {}
+
+  /* ----------- Overridable periodic code ----------------- */
+
+  private boolean m_rpFirstRun = true;
+
+  /** Periodic code for all robot modes should go here. */
+  public void robotPeriodic() {
+    if (m_rpFirstRun) {
+      System.out.println("Default robotPeriodic() method... Override me!");
+      m_rpFirstRun = false;
+    }
+  }
+
+  private boolean m_spFirstRun = true;
+
+  /**
+   * Periodic simulation code should go here.
+   *
+   * <p>This function is called in a simulated robot after user code executes.
+   */
+  public void simulationPeriodic() {
+    if (m_spFirstRun) {
+      System.out.println("Default simulationPeriodic() method... Override me!");
+      m_spFirstRun = false;
+    }
+  }
+
+  private boolean m_dpFirstRun = true;
+
+  /** Periodic code for disabled mode should go here. */
+  public void disabledPeriodic() {
+    if (m_dpFirstRun) {
+      System.out.println("Default disabledPeriodic() method... Override me!");
+      m_dpFirstRun = false;
+    }
+  }
+
+  private boolean m_apFirstRun = true;
+
+  /** Periodic code for autonomous mode should go here. */
+  public void autonomousPeriodic() {
+    if (m_apFirstRun) {
+      System.out.println("Default autonomousPeriodic() method... Override me!");
+      m_apFirstRun = false;
+    }
+  }
+
+  private boolean m_tpFirstRun = true;
+
+  /** Periodic code for teleop mode should go here. */
+  public void teleopPeriodic() {
+    if (m_tpFirstRun) {
+      System.out.println("Default teleopPeriodic() method... Override me!");
+      m_tpFirstRun = false;
+    }
+  }
+
+  private boolean m_tmpFirstRun = true;
+
+  /** Periodic code for test mode should go here. */
+  public void testPeriodic() {
+    if (m_tmpFirstRun) {
+      System.out.println("Default testPeriodic() method... Override me!");
+      m_tmpFirstRun = false;
+    }
+  }
+
+  /**
+   * Exit code for disabled mode should go here.
+   *
+   * <p>Users should override this method for code which will be called each time the robot exits
+   * disabled mode.
+   */
+  public void disabledExit() {}
+
+  /**
+   * Exit code for autonomous mode should go here.
+   *
+   * <p>Users should override this method for code which will be called each time the robot exits
+   * autonomous mode.
+   */
+  public void autonomousExit() {}
+
+  /**
+   * Exit code for teleop mode should go here.
+   *
+   * <p>Users should override this method for code which will be called each time the robot exits
+   * teleop mode.
+   */
+  public void teleopExit() {}
+
+  /**
+   * Exit code for test mode should go here.
+   *
+   * <p>Users should override this method for code which will be called each time the robot exits
+   * test mode.
+   */
+  public void testExit() {}
+
+  /**
+   * Enables or disables flushing NetworkTables every loop iteration. By default, this is enabled.
+   *
+   * @param enabled True to enable, false to disable
+   * @deprecated Deprecated without replacement.
+   */
+  @Deprecated(forRemoval = true, since = "2025")
+  public void setNetworkTablesFlushEnabled(boolean enabled) {
+    m_ntFlushEnabled = enabled;
+  }
+
+  /**
+   * Gets time period between calls to Periodic() functions.
+   *
+   * @return The time period between calls to Periodic() functions.
+   */
+  public double getPeriod() {
+    return m_period;
+  }
+
+  /** Loop function. */
+  protected void loopFunc() {
+    DriverStation.refreshData();
+    m_watchdog.reset();
+
+    m_word.refresh();
+
+    // Get current mode
+    Mode mode = Mode.kNone;
+    if (m_word.isDisabled()) {
+      mode = Mode.kDisabled;
+    } else if (m_word.isAutonomous()) {
+      mode = Mode.kAutonomous;
+    } else if (m_word.isTeleop()) {
+      mode = Mode.kTeleop;
+    } else if (m_word.isTest()) {
+      mode = Mode.kTest;
+    }
+
+    if (!m_calledDsConnected && m_word.isDSAttached()) {
+      m_calledDsConnected = true;
+      driverStationConnected();
+    }
+
+    // If mode changed, call mode exit and entry functions
+    if (m_lastMode != mode) {
+      // Call last mode's exit function
+      switch (m_lastMode) {
+        case kDisabled -> disabledExit();
+        case kAutonomous -> autonomousExit();
+        case kTeleop -> teleopExit();
+        case kTest -> testExit();
+        default -> {
+          // NOP
+        }
+      }
+
+      // Call current mode's entry function
+      switch (mode) {
+        case kDisabled -> {
+          disabledInit();
+          m_watchdog.addEpoch("disabledInit()");
+        }
+        case kAutonomous -> {
+          autonomousInit();
+          m_watchdog.addEpoch("autonomousInit()");
+        }
+        case kTeleop -> {
+          teleopInit();
+          m_watchdog.addEpoch("teleopInit()");
+        }
+        case kTest -> {
+          testInit();
+          m_watchdog.addEpoch("testInit()");
+        }
+        default -> {
+          // NOP
+        }
+      }
+
+      m_lastMode = mode;
+    }
+
+    // Call the appropriate function depending upon the current robot mode
+    switch (mode) {
+      case kDisabled -> {
+        DriverStationJNI.observeUserProgramDisabled();
+        disabledPeriodic();
+        m_watchdog.addEpoch("disabledPeriodic()");
+      }
+      case kAutonomous -> {
+        DriverStationJNI.observeUserProgramAutonomous();
+        autonomousPeriodic();
+        m_watchdog.addEpoch("autonomousPeriodic()");
+      }
+      case kTeleop -> {
+        DriverStationJNI.observeUserProgramTeleop();
+        teleopPeriodic();
+        m_watchdog.addEpoch("teleopPeriodic()");
+      }
+      case kTest -> {
+        DriverStationJNI.observeUserProgramTest();
+        testPeriodic();
+        m_watchdog.addEpoch("testPeriodic()");
+      }
+      default -> {
+        // NOP
+      }
+    }
+
+    robotPeriodic();
+    m_watchdog.addEpoch("robotPeriodic()");
+
+    SmartDashboard.updateValues();
+    m_watchdog.addEpoch("SmartDashboard.updateValues()");
+
+    if (isSimulation()) {
+      HAL.simPeriodicBefore();
+      simulationPeriodic();
+      HAL.simPeriodicAfter();
+      m_watchdog.addEpoch("simulationPeriodic()");
+    }
+
+    m_watchdog.disable();
+
+    // Flush NetworkTables
+    if (m_ntFlushEnabled) {
+      NetworkTableInstance.getDefault().flushLocal();
+    }
+
+    // Warn on loop time overruns
+    if (m_watchdog.isExpired()) {
+      m_watchdog.printEpochs();
+    }
+  }
+
+  /** Prints list of epochs added so far and their times. */
+  public void printWatchdogEpochs() {
+    m_watchdog.printEpochs();
+  }
+
+  private void printLoopOverrunMessage() {
+    DriverStation.reportWarning("Loop time of " + m_period + "s overrun\n", false);
+  }
+}

wpilibj/src/main/java/org/wpilib/opmode/RobotBase.java

@@ -0,0 +1,400 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.cameraserver.CameraServerShared;
+import edu.wpi.first.cameraserver.CameraServerSharedStore;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.math.MathShared;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.networktables.MultiSubscriber;
+import edu.wpi.first.networktables.NetworkTableEvent;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.wpilibj.util.WPILibVersion;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Supplier;
+
+/**
+ * Implement a Robot Program framework. The RobotBase class is intended to be subclassed to create a
+ * robot program. The user must implement {@link #startCompetition()}, which will be called once and
+ * is not expected to exit. The user must also implement {@link #endCompetition()}, which signals to
+ * the code in {@link #startCompetition()} that it should exit.
+ *
+ * <p>It is not recommended to subclass this class directly - instead subclass IterativeRobotBase or
+ * TimedRobot.
+ */
+public abstract class RobotBase implements AutoCloseable {
+  /** The ID of the main Java thread. */
+  // This is usually 1, but it is best to make sure
+  private static long m_threadId = -1;
+
+  private final MultiSubscriber m_suball;
+
+  private final int m_connListenerHandle;
+
+  private static void setupCameraServerShared() {
+    CameraServerShared shared =
+        new CameraServerShared() {
+          @Override
+          public void reportUsage(String resource, String data) {
+            HAL.reportUsage(resource, data);
+          }
+
+          @Override
+          public void reportDriverStationError(String error) {
+            DriverStation.reportError(error, true);
+          }
+
+          @Override
+          public Long getRobotMainThreadId() {
+            return RobotBase.getMainThreadId();
+          }
+
+          @Override
+          public boolean isRoboRIO() {
+            return !RobotBase.isSimulation();
+          }
+        };
+
+    CameraServerSharedStore.setCameraServerShared(shared);
+  }
+
+  private static void setupMathShared() {
+    MathSharedStore.setMathShared(
+        new MathShared() {
+          @Override
+          public void reportError(String error, StackTraceElement[] stackTrace) {
+            DriverStation.reportError(error, stackTrace);
+          }
+
+          @Override
+          public void reportUsage(String resource, String data) {
+            HAL.reportUsage(resource, data);
+          }
+
+          @Override
+          public double getTimestamp() {
+            return Timer.getTimestamp();
+          }
+        });
+  }
+
+  /**
+   * Constructor for a generic robot program. User code can be placed in the constructor that runs
+   * before the Autonomous or Operator Control period starts. The constructor will run to completion
+   * before Autonomous is entered.
+   *
+   * <p>This must be used to ensure that the communications code starts. In the future it would be
+   * nice to put this code into its own task that loads on boot so ensure that it runs.
+   */
+  protected RobotBase() {
+    final NetworkTableInstance inst = NetworkTableInstance.getDefault();
+    m_threadId = Thread.currentThread().threadId();
+    setupCameraServerShared();
+    setupMathShared();
+    // subscribe to "" to force persistent values to propagate to local
+    m_suball = new MultiSubscriber(inst, new String[] {""});
+    if (!isSimulation()) {
+      inst.startServer("/home/systemcore/networktables.json");
+    } else {
+      inst.startServer();
+    }
+
+    // wait for the NT server to actually start
+    try {
+      int count = 0;
+      while (inst.getNetworkMode().contains(NetworkTableInstance.NetworkMode.kStarting)) {
+        Thread.sleep(10);
+        count++;
+        if (count > 100) {
+          throw new InterruptedException();
+        }
+      }
+    } catch (InterruptedException ex) {
+      System.err.println("timed out while waiting for NT server to start");
+    }
+
+    m_connListenerHandle =
+        inst.addConnectionListener(
+            false,
+            event -> {
+              if (event.is(NetworkTableEvent.Kind.kConnected)) {
+                HAL.reportUsage("NT/" + event.connInfo.remote_id, "");
+              }
+            });
+  }
+
+  /**
+   * Returns the main thread ID.
+   *
+   * @return The main thread ID.
+   */
+  public static long getMainThreadId() {
+    return m_threadId;
+  }
+
+  @Override
+  public void close() {
+    m_suball.close();
+    NetworkTableInstance.getDefault().removeListener(m_connListenerHandle);
+  }
+
+  /**
+   * Get the current runtime type.
+   *
+   * @return Current runtime type.
+   */
+  public static RuntimeType getRuntimeType() {
+    return RuntimeType.getValue(HALUtil.getHALRuntimeType());
+  }
+
+  /**
+   * Get if the robot is a simulation.
+   *
+   * @return If the robot is running in simulation.
+   */
+  public static boolean isSimulation() {
+    return getRuntimeType() == RuntimeType.kSimulation;
+  }
+
+  /**
+   * Get if the robot is real.
+   *
+   * @return If the robot is running in the real world.
+   */
+  public static boolean isReal() {
+    RuntimeType runtimeType = getRuntimeType();
+    return runtimeType == RuntimeType.kSystemCore;
+  }
+
+  /**
+   * Determine if the Robot is currently disabled.
+   *
+   * @return True if the Robot is currently disabled by the Driver Station.
+   */
+  public boolean isDisabled() {
+    return DriverStation.isDisabled();
+  }
+
+  /**
+   * Determine if the Robot is currently enabled.
+   *
+   * @return True if the Robot is currently enabled by the Driver Station.
+   */
+  public boolean isEnabled() {
+    return DriverStation.isEnabled();
+  }
+
+  /**
+   * Determine if the robot is currently in Autonomous mode as determined by the Driver Station.
+   *
+   * @return True if the robot is currently operating Autonomously.
+   */
+  public boolean isAutonomous() {
+    return DriverStation.isAutonomous();
+  }
+
+  /**
+   * Determine if the robot is currently in Autonomous mode and enabled as determined by the Driver
+   * Station.
+   *
+   * @return True if the robot is currently operating autonomously while enabled.
+   */
+  public boolean isAutonomousEnabled() {
+    return DriverStation.isAutonomousEnabled();
+  }
+
+  /**
+   * Determine if the robot is currently in Test mode as determined by the Driver Station.
+   *
+   * @return True if the robot is currently operating in Test mode.
+   */
+  public boolean isTest() {
+    return DriverStation.isTest();
+  }
+
+  /**
+   * Determine if the robot is current in Test mode and enabled as determined by the Driver Station.
+   *
+   * @return True if the robot is currently operating in Test mode while enabled.
+   */
+  public boolean isTestEnabled() {
+    return DriverStation.isTestEnabled();
+  }
+
+  /**
+   * Determine if the robot is currently in Operator Control mode as determined by the Driver
+   * Station.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode.
+   */
+  public boolean isTeleop() {
+    return DriverStation.isTeleop();
+  }
+
+  /**
+   * Determine if the robot is currently in Operator Control mode and enabled as determined by the
+   * Driver Station.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode while enabled.
+   */
+  public boolean isTeleopEnabled() {
+    return DriverStation.isTeleopEnabled();
+  }
+
+  /**
+   * Start the main robot code. This function will be called once and should not exit until
+   * signalled by {@link #endCompetition()}
+   */
+  public abstract void startCompetition();
+
+  /** Ends the main loop in {@link #startCompetition()}. */
+  public abstract void endCompetition();
+
+  private static final ReentrantLock m_runMutex = new ReentrantLock();
+  private static RobotBase m_robotCopy;
+  private static boolean m_suppressExitWarning;
+
+  /** Run the robot main loop. */
+  private static <T extends RobotBase> void runRobot(Supplier<T> robotSupplier) {
+    System.out.println("********** Robot program starting **********");
+
+    T robot;
+    try {
+      robot = robotSupplier.get();
+    } catch (Throwable throwable) {
+      Throwable cause = throwable.getCause();
+      if (cause != null) {
+        throwable = cause;
+      }
+      String robotName = "Unknown";
+      StackTraceElement[] elements = throwable.getStackTrace();
+      if (elements.length > 0) {
+        robotName = elements[0].getClassName();
+      }
+      DriverStation.reportError(
+          "Unhandled exception instantiating robot " + robotName + " " + throwable, elements);
+      DriverStation.reportError(
+          "The robot program quit unexpectedly."
+              + " This is usually due to a code error.\n"
+              + "  The above stacktrace can help determine where the error occurred.\n"
+              + "  See https://wpilib.org/stacktrace for more information.\n",
+          false);
+      DriverStation.reportError("Could not instantiate robot " + robotName + "!", false);
+      return;
+    }
+
+    m_runMutex.lock();
+    m_robotCopy = robot;
+    m_runMutex.unlock();
+
+    boolean errorOnExit = false;
+    try {
+      robot.startCompetition();
+    } catch (Throwable throwable) {
+      Throwable cause = throwable.getCause();
+      if (cause != null) {
+        throwable = cause;
+      }
+      DriverStation.reportError("Unhandled exception: " + throwable, throwable.getStackTrace());
+      errorOnExit = true;
+    } finally {
+      m_runMutex.lock();
+      boolean suppressExitWarning = m_suppressExitWarning;
+      m_runMutex.unlock();
+      if (!suppressExitWarning) {
+        // startCompetition never returns unless exception occurs....
+        DriverStation.reportWarning(
+            "The robot program quit unexpectedly."
+                + " This is usually due to a code error.\n"
+                + "  The above stacktrace can help determine where the error occurred.\n"
+                + "  See https://wpilib.org/stacktrace for more information.",
+            false);
+        if (errorOnExit) {
+          DriverStation.reportError(
+              "The startCompetition() method (or methods called by it) should have "
+                  + "handled the exception above.",
+              false);
+        } else {
+          DriverStation.reportError("Unexpected return from startCompetition() method.", false);
+        }
+      }
+    }
+  }
+
+  /**
+   * Suppress the "The robot program quit unexpectedly." message.
+   *
+   * @param value True if exit warning should be suppressed.
+   */
+  public static void suppressExitWarning(boolean value) {
+    m_runMutex.lock();
+    m_suppressExitWarning = value;
+    m_runMutex.unlock();
+  }
+
+  /**
+   * Starting point for the applications.
+   *
+   * @param <T> Robot subclass.
+   * @param robotSupplier Function that returns an instance of the robot subclass.
+   */
+  public static <T extends RobotBase> void startRobot(Supplier<T> robotSupplier) {
+    // Check that the MSVC runtime is valid.
+    WPIUtilJNI.checkMsvcRuntime();
+
+    if (!HAL.initialize(500, 0)) {
+      throw new IllegalStateException("Failed to initialize. Terminating");
+    }
+
+    // Force refresh DS data
+    DriverStation.refreshData();
+
+    HAL.reportUsage("Language", "Java");
+    HAL.reportUsage("WPILibVersion", WPILibVersion.Version);
+
+    if (!Notifier.setHALThreadPriority(true, 40)) {
+      DriverStation.reportWarning("Setting HAL Notifier RT priority to 40 failed", false);
+    }
+
+    if (HAL.hasMain()) {
+      Thread thread =
+          new Thread(
+              () -> {
+                runRobot(robotSupplier);
+                HAL.exitMain();
+              },
+              "robot main");
+      thread.setDaemon(true);
+      thread.start();
+      HAL.runMain();
+      suppressExitWarning(true);
+      m_runMutex.lock();
+      RobotBase robot = m_robotCopy;
+      m_runMutex.unlock();
+      if (robot != null) {
+        robot.endCompetition();
+      }
+      try {
+        thread.join(1000);
+      } catch (InterruptedException ex) {
+        Thread.currentThread().interrupt();
+      }
+    } else {
+      runRobot(robotSupplier);
+    }
+
+    // On RIO, this will just terminate rather than shutting down cleanly (it's a no-op in sim).
+    // It's not worth the risk of hanging on shutdown when we want the code to restart as quickly
+    // as possible.
+    HAL.terminate();
+
+    HAL.shutdown();
+
+    System.exit(0);
+  }
+}

wpilibj/src/main/java/org/wpilib/opmode/RobotState.java

@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+/** Robot state utility functions. */
+public final class RobotState {
+  /**
+   * Returns true if the robot is disabled.
+   *
+   * @return True if the robot is disabled.
+   */
+  public static boolean isDisabled() {
+    return DriverStation.isDisabled();
+  }
+
+  /**
+   * Returns true if the robot is enabled.
+   *
+   * @return True if the robot is enabled.
+   */
+  public static boolean isEnabled() {
+    return DriverStation.isEnabled();
+  }
+
+  /**
+   * Returns true if the robot is E-stopped.
+   *
+   * @return True if the robot is E-stopped.
+   */
+  public static boolean isEStopped() {
+    return DriverStation.isEStopped();
+  }
+
+  /**
+   * Returns true if the robot is in teleop mode.
+   *
+   * @return True if the robot is in teleop mode.
+   */
+  public static boolean isTeleop() {
+    return DriverStation.isTeleop();
+  }
+
+  /**
+   * Returns true if the robot is in autonomous mode.
+   *
+   * @return True if the robot is in autonomous mode.
+   */
+  public static boolean isAutonomous() {
+    return DriverStation.isAutonomous();
+  }
+
+  /**
+   * Returns true if the robot is in test mode.
+   *
+   * @return True if the robot is in test mode.
+   */
+  public static boolean isTest() {
+    return DriverStation.isTest();
+  }
+
+  private RobotState() {}
+}

wpilibj/src/main/java/org/wpilib/opmode/TimedRobot.java

@@ -0,0 +1,246 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.NotifierJNI;
+import edu.wpi.first.units.measure.Frequency;
+import edu.wpi.first.units.measure.Time;
+import java.util.PriorityQueue;
+
+/**
+ * TimedRobot implements the IterativeRobotBase robot program framework.
+ *
+ * <p>The TimedRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ * <p>periodic() functions from the base class are called on an interval by a Notifier instance.
+ */
+public class TimedRobot extends IterativeRobotBase {
+  @SuppressWarnings("MemberName")
+  static class Callback implements Comparable<Callback> {
+    public Runnable func;
+    public long period;
+    public long expirationTime;
+
+    /**
+     * Construct a callback container.
+     *
+     * @param func The callback to run.
+     * @param startTime The common starting point for all callback scheduling in microseconds.
+     * @param period The period at which to run the callback in microseconds.
+     * @param offset The offset from the common starting time in microseconds.
+     */
+    Callback(Runnable func, long startTime, long period, long offset) {
+      this.func = func;
+      this.period = period;
+      this.expirationTime =
+          startTime
+              + offset
+              + this.period
+              + (RobotController.getFPGATime() - startTime) / this.period * this.period;
+    }
+
+    @Override
+    public boolean equals(Object rhs) {
+      return rhs instanceof Callback callback && expirationTime == callback.expirationTime;
+    }
+
+    @Override
+    public int hashCode() {
+      return Long.hashCode(expirationTime);
+    }
+
+    @Override
+    public int compareTo(Callback rhs) {
+      // Elements with sooner expiration times are sorted as lesser. The head of
+      // Java's PriorityQueue is the least element.
+      return Long.compare(expirationTime, rhs.expirationTime);
+    }
+  }
+
+  /** Default loop period. */
+  public static final double kDefaultPeriod = 0.02;
+
+  // The C pointer to the notifier object. We don't use it directly, it is
+  // just passed to the JNI bindings.
+  private final int m_notifier = NotifierJNI.initializeNotifier();
+
+  private long m_startTimeUs;
+  private long m_loopStartTimeUs;
+
+  private final PriorityQueue<Callback> m_callbacks = new PriorityQueue<>();
+
+  /** Constructor for TimedRobot. */
+  protected TimedRobot() {
+    this(kDefaultPeriod);
+  }
+
+  /**
+   * Constructor for TimedRobot.
+   *
+   * @param period The period of the robot loop function.
+   */
+  protected TimedRobot(double period) {
+    super(period);
+    m_startTimeUs = RobotController.getFPGATime();
+    addPeriodic(this::loopFunc, period);
+    NotifierJNI.setNotifierName(m_notifier, "TimedRobot");
+
+    HAL.reportUsage("Framework", "TimedRobot");
+  }
+
+  /**
+   * Constructor for TimedRobot.
+   *
+   * @param period The period of the robot loop function.
+   */
+  protected TimedRobot(Time period) {
+    this(period.in(Seconds));
+  }
+
+  /**
+   * Constructor for TimedRobot.
+   *
+   * @param frequency The frequency of the robot loop function.
+   */
+  protected TimedRobot(Frequency frequency) {
+    this(frequency.asPeriod());
+  }
+
+  @Override
+  public void close() {
+    NotifierJNI.stopNotifier(m_notifier);
+    NotifierJNI.cleanNotifier(m_notifier);
+  }
+
+  /** Provide an alternate "main loop" via startCompetition(). */
+  @Override
+  public void startCompetition() {
+    if (isSimulation()) {
+      simulationInit();
+    }
+
+    // Tell the DS that the robot is ready to be enabled
+    System.out.println("********** Robot program startup complete **********");
+    DriverStationJNI.observeUserProgramStarting();
+
+    // Loop forever, calling the appropriate mode-dependent function
+    while (true) {
+      // We don't have to check there's an element in the queue first because
+      // there's always at least one (the constructor adds one). It's reenqueued
+      // at the end of the loop.
+      var callback = m_callbacks.poll();
+
+      NotifierJNI.updateNotifierAlarm(m_notifier, callback.expirationTime);
+
+      long currentTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
+      if (currentTime == 0) {
+        break;
+      }
+
+      m_loopStartTimeUs = RobotController.getFPGATime();
+
+      callback.func.run();
+
+      // Increment the expiration time by the number of full periods it's behind
+      // plus one to avoid rapid repeat fires from a large loop overrun. We
+      // assume currentTime ≥ expirationTime rather than checking for it since
+      // the callback wouldn't be running otherwise.
+      callback.expirationTime +=
+          callback.period
+              + (currentTime - callback.expirationTime) / callback.period * callback.period;
+      m_callbacks.add(callback);
+
+      // Process all other callbacks that are ready to run
+      while (m_callbacks.peek().expirationTime <= currentTime) {
+        callback = m_callbacks.poll();
+
+        callback.func.run();
+
+        callback.expirationTime +=
+            callback.period
+                + (currentTime - callback.expirationTime) / callback.period * callback.period;
+        m_callbacks.add(callback);
+      }
+    }
+  }
+
+  /** Ends the main loop in startCompetition(). */
+  @Override
+  public void endCompetition() {
+    NotifierJNI.stopNotifier(m_notifier);
+  }
+
+  /**
+   * Return the system clock time in micrseconds for the start of the current periodic loop. This is
+   * in the same time base as Timer.getFPGATimestamp(), but is stable through a loop. It is updated
+   * at the beginning of every periodic callback (including the normal periodic loop).
+   *
+   * @return Robot running time in microseconds, as of the start of the current periodic function.
+   */
+  public long getLoopStartTime() {
+    return m_loopStartTimeUs;
+  }
+
+  /**
+   * Add a callback to run at a specific period.
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback The callback to run.
+   * @param period The period at which to run the callback in seconds.
+   */
+  public final void addPeriodic(Runnable callback, double period) {
+    m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0));
+  }
+
+  /**
+   * Add a callback to run at a specific period with a starting time offset.
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback The callback to run.
+   * @param period The period at which to run the callback in seconds.
+   * @param offset The offset from the common starting time in seconds. This is useful for
+   *     scheduling a callback in a different timeslot relative to TimedRobot.
+   */
+  public final void addPeriodic(Runnable callback, double period, double offset) {
+    m_callbacks.add(
+        new Callback(callback, m_startTimeUs, (long) (period * 1e6), (long) (offset * 1e6)));
+  }
+
+  /**
+   * Add a callback to run at a specific period.
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback The callback to run.
+   * @param period The period at which to run the callback.
+   */
+  public final void addPeriodic(Runnable callback, Time period) {
+    addPeriodic(callback, period.in(Seconds));
+  }
+
+  /**
+   * Add a callback to run at a specific period with a starting time offset.
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback The callback to run.
+   * @param period The period at which to run the callback.
+   * @param offset The offset from the common starting time. This is useful for scheduling a
+   *     callback in a different timeslot relative to TimedRobot.
+   */
+  public final void addPeriodic(Runnable callback, Time period, Time offset) {
+    addPeriodic(callback, period.in(Seconds), offset.in(Seconds));
+  }
+}

wpilibj/src/main/java/org/wpilib/opmode/TimesliceRobot.java

@@ -0,0 +1,116 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * TimesliceRobot extends the TimedRobot robot program framework to provide timeslice scheduling of
+ * periodic functions.
+ *
+ * <p>The TimesliceRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ * <p>This class schedules robot operations serially in a timeslice format. TimedRobot's periodic
+ * functions are the first in the timeslice table with 0 ms offset and 20 ms period. You can
+ * schedule additional controller periodic functions at a shorter period (5 ms by default). You give
+ * each one a timeslice duration, then they're run sequentially. The main benefit of this approach
+ * is consistent starting times for each controller periodic, which can make odometry and estimators
+ * more accurate and controller outputs change more consistently.
+ *
+ * <p>Here's an example of measured subsystem durations and their timeslice allocations:
+ *
+ * <table>
+ *   <tr>
+ *     <td><b>Subsystem</b></td>
+ *     <td><b>Duration (ms)</b></td>
+ *     <td><b>Allocation (ms)</b></td>
+ *   </tr>
+ *   <tr>
+ *     <td><b>Total</b></td>
+ *     <td>5.0</td>
+ *     <td>5.0</td>
+ *   </tr>
+ *   <tr>
+ *     <td>TimedRobot</td>
+ *     <td>?</td>
+ *     <td>2.0</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Drivetrain</td>
+ *     <td>1.32</td>
+ *     <td>1.5</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Flywheel</td>
+ *     <td>0.6</td>
+ *     <td>0.7</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Turret</td>
+ *     <td>0.6</td>
+ *     <td>0.8</td>
+ *   </tr>
+ *   <tr>
+ *     <td><b>Free</b></td>
+ *     <td>0.0</td>
+ *     <td>N/A</td>
+ *   </tr>
+ * </table>
+ *
+ * <p>Since TimedRobot periodic functions only run every 20ms, that leaves a 2 ms empty spot in the
+ * allocation table for three of the four 5 ms cycles comprising 20 ms. That's OK because the OS
+ * needs time to do other things.
+ *
+ * <p>If the robot periodic functions and the controller periodic functions have a lot of scheduling
+ * jitter that cause them to occasionally overlap with later timeslices, consider giving the main
+ * robot thread a real-time priority using {@link Threads#setCurrentThreadPriority(boolean,int)}. An
+ * RT priority of 15 is a reasonable choice.
+ *
+ * <p>If you do enable RT though, <i>make sure your periodic functions do not block</i>. If they do,
+ * the operating system will lock up, and you'll have to boot the roboRIO into safe mode and delete
+ * the robot program to recover.
+ */
+public class TimesliceRobot extends TimedRobot {
+  /**
+   * Constructor for TimesliceRobot.
+   *
+   * @param robotPeriodicAllocation The allocation in seconds to give the TimesliceRobot periodic
+   *     functions.
+   * @param controllerPeriod The controller period in seconds. The sum of all scheduler allocations
+   *     should be less than or equal to this value.
+   */
+  public TimesliceRobot(double robotPeriodicAllocation, double controllerPeriod) {
+    m_nextOffset = robotPeriodicAllocation;
+    m_controllerPeriod = controllerPeriod;
+  }
+
+  /**
+   * Schedule a periodic function with the constructor's controller period and the given allocation.
+   * The function's runtime allocation will be placed after the end of the previous one's.
+   *
+   * <p>If a call to this function makes the allocations exceed the controller period, an exception
+   * will be thrown since that means the TimesliceRobot periodic functions and the given function
+   * will have conflicting timeslices.
+   *
+   * @param func Function to schedule.
+   * @param allocation The function's runtime allocation in seconds out of the controller period.
+   */
+  public void schedule(Runnable func, double allocation) {
+    if (m_nextOffset + allocation > m_controllerPeriod) {
+      throw new IllegalStateException(
+          "Function scheduled at offset "
+              + m_nextOffset
+              + " with allocation "
+              + allocation
+              + " exceeded controller period of "
+              + m_controllerPeriod
+              + "\n");
+    }
+
+    addPeriodic(func, m_controllerPeriod, m_nextOffset);
+    m_nextOffset += allocation;
+  }
+
+  private double m_nextOffset;
+  private final double m_controllerPeriod;
+}

wpilibj/src/main/java/org/wpilib/package.html

@@ -0,0 +1,30 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html>
+  <head>
+    <title>
+      WPI Robotics library
+    </title>
+    <meta http-equiv="Content-Type" content="text/html; charset=utf-8">
+  </head>
+  <body>
+    The WPI Robotics library (WPILibJ) is a set of Java classes that interfaces
+    to the hardware in the FRC control system and your robot. There are classes
+    to handle sensors, motors, the driver station, and a number of other
+    utility functions like timing and field management. The library is designed
+    to:
+    <ul>
+      <li>Deal with all the low level interfacing to these components, so you
+      can concentrate on solving this year's "robot problem". This is a
+      philosophical decision to let you focus on the higher-level design of
+      your robot rather than deal with the details of the processor and the
+      operating system.
+      </li>
+      <li>Understand everything at all levels by making the full source code of
+      the library available. You can study (and modify) the algorithms used by
+      the gyro class for oversampling and integration of the input signal or
+      just ask the class for the current robot heading. You can work at any
+      level.
+      </li>
+    </ul>
+  </body>
+</html>

wpilibj/src/main/java/org/wpilib/simulation/ADXL345Sim.java

@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.ADXL345_I2C;
+import java.util.Objects;
+
+/** Class to control a simulated ADXL345. */
+public class ADXL345Sim {
+  private SimDouble m_simX;
+  private SimDouble m_simY;
+  private SimDouble m_simZ;
+
+  /**
+   * Constructor.
+   *
+   * @param device The device to simulate
+   */
+  public ADXL345Sim(ADXL345_I2C device) {
+    SimDeviceSim simDevice =
+        new SimDeviceSim(
+            "Accel:ADXL345_I2C" + "[" + device.getPort() + "," + device.getDeviceAddress() + "]");
+    initSim(simDevice);
+  }
+
+  private void initSim(SimDeviceSim simDevice) {
+    Objects.requireNonNull(simDevice);
+
+    m_simX = simDevice.getDouble("x");
+    m_simY = simDevice.getDouble("y");
+    m_simZ = simDevice.getDouble("z");
+
+    Objects.requireNonNull(m_simX);
+    Objects.requireNonNull(m_simY);
+    Objects.requireNonNull(m_simZ);
+  }
+
+  /**
+   * Sets the X acceleration.
+   *
+   * @param accel The X acceleration.
+   */
+  public void setX(double accel) {
+    m_simX.set(accel);
+  }
+
+  /**
+   * Sets the Y acceleration.
+   *
+   * @param accel The Y acceleration.
+   */
+  public void setY(double accel) {
+    m_simY.set(accel);
+  }
+
+  /**
+   * Sets the Z acceleration.
+   *
+   * @param accel The Z acceleration.
+   */
+  public void setZ(double accel) {
+    m_simZ.set(accel);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/AddressableLEDSim.java

@@ -0,0 +1,178 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.AddressableLEDDataJNI;
+import edu.wpi.first.hal.simulation.ConstBufferCallback;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.AddressableLED;
+
+/** Class to control a simulated addressable LED. */
+public class AddressableLEDSim {
+  private final int m_channel;
+
+  /**
+   * Constructs an addressable LED for a specific channel.
+   *
+   * @param channel output channel
+   */
+  public AddressableLEDSim(int channel) {
+    m_channel = channel;
+  }
+
+  /**
+   * Constructs from an AddressableLED object.
+   *
+   * @param addressableLED AddressableLED to simulate
+   */
+  public AddressableLEDSim(AddressableLED addressableLED) {
+    m_channel = addressableLED.getChannel();
+  }
+
+  /**
+   * Register a callback on the Initialized property.
+   *
+   * @param callback the callback that will be called whenever the Initialized property is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AddressableLEDDataJNI.registerInitializedCallback(m_channel, callback, initialNotify);
+    return new CallbackStore(m_channel, uid, AddressableLEDDataJNI::cancelInitializedCallback);
+  }
+
+  /**
+   * Check if initialized.
+   *
+   * @return true if initialized
+   */
+  public boolean getInitialized() {
+    return AddressableLEDDataJNI.getInitialized(m_channel);
+  }
+
+  /**
+   * Change the Initialized value of the LED strip.
+   *
+   * @param initialized the new value
+   */
+  public void setInitialized(boolean initialized) {
+    AddressableLEDDataJNI.setInitialized(m_channel, initialized);
+  }
+
+  /**
+   * Register a callback on the start.
+   *
+   * @param callback the callback that will be called whenever the start is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerStartCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AddressableLEDDataJNI.registerStartCallback(m_channel, callback, initialNotify);
+    return new CallbackStore(m_channel, uid, AddressableLEDDataJNI::cancelStartCallback);
+  }
+
+  /**
+   * Get the start.
+   *
+   * @return the start
+   */
+  public int getStart() {
+    return AddressableLEDDataJNI.getStart(m_channel);
+  }
+
+  /**
+   * Change the start.
+   *
+   * @param start the new start
+   */
+  public void setStart(int start) {
+    AddressableLEDDataJNI.setStart(m_channel, start);
+  }
+
+  /**
+   * Register a callback on the length.
+   *
+   * @param callback the callback that will be called whenever the length is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerLengthCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AddressableLEDDataJNI.registerLengthCallback(m_channel, callback, initialNotify);
+    return new CallbackStore(m_channel, uid, AddressableLEDDataJNI::cancelLengthCallback);
+  }
+
+  /**
+   * Get the length of the LED strip.
+   *
+   * @return the length
+   */
+  public int getLength() {
+    return AddressableLEDDataJNI.getLength(m_channel);
+  }
+
+  /**
+   * Change the length of the LED strip.
+   *
+   * @param length the new value
+   */
+  public void setLength(int length) {
+    AddressableLEDDataJNI.setLength(m_channel, length);
+  }
+
+  /**
+   * Register a callback on the LED data.
+   *
+   * @param callback the callback that will be called whenever the LED data is changed
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerDataCallback(ConstBufferCallback callback) {
+    int uid = AddressableLEDDataJNI.registerDataCallback(callback);
+    return new CallbackStore(uid, AddressableLEDDataJNI::cancelDataCallback);
+  }
+
+  /**
+   * Get the LED data.
+   *
+   * @return the LED data
+   */
+  public byte[] getData() {
+    return getGlobalData(getStart(), getLength());
+  }
+
+  /**
+   * Change the LED data.
+   *
+   * @param data the new data
+   */
+  public void setData(byte[] data) {
+    setGlobalData(getStart(), data);
+  }
+
+  /**
+   * Get the global LED data.
+   *
+   * @param start start, in LEDs
+   * @param length length, in LEDs
+   * @return the LED data
+   */
+  public static byte[] getGlobalData(int start, int length) {
+    return AddressableLEDDataJNI.getData(start, length);
+  }
+
+  /**
+   * Change the global LED data.
+   *
+   * @param start start, in LEDs
+   * @param data the new data
+   */
+  public static void setGlobalData(int start, byte[] data) {
+    AddressableLEDDataJNI.setData(start, data);
+  }
+
+  /** Reset all simulation data for this LED object. */
+  public void resetData() {
+    AddressableLEDDataJNI.resetData(m_channel);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/AnalogEncoderSim.java

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.AnalogEncoder;
+
+/** Class to control a simulated analog encoder. */
+public class AnalogEncoderSim {
+  private final SimDouble m_simPosition;
+
+  /**
+   * Constructs from an AnalogEncoder object.
+   *
+   * @param encoder AnalogEncoder to simulate
+   */
+  public AnalogEncoderSim(AnalogEncoder encoder) {
+    SimDeviceSim wrappedSimDevice =
+        new SimDeviceSim("AnalogEncoder" + "[" + encoder.getChannel() + "]");
+    m_simPosition = wrappedSimDevice.getDouble("Position");
+  }
+
+  /**
+   * Set the position.
+   *
+   * @param value The position.
+   */
+  public void set(double value) {
+    m_simPosition.set(value);
+  }
+
+  /**
+   * Get the simulated position.
+   *
+   * @return The simulated position.
+   */
+  public double get() {
+    return m_simPosition.get();
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/AnalogInputSim.java

@@ -0,0 +1,158 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.AnalogInDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.AnalogInput;
+
+/** Class to control a simulated analog input. */
+public class AnalogInputSim {
+  private final int m_index;
+
+  /**
+   * Constructs from an AnalogInput object.
+   *
+   * @param analogInput AnalogInput to simulate
+   */
+  public AnalogInputSim(AnalogInput analogInput) {
+    m_index = analogInput.getChannel();
+  }
+
+  /**
+   * Constructs from an analog input channel number.
+   *
+   * @param channel Channel number
+   */
+  public AnalogInputSim(int channel) {
+    m_index = channel;
+  }
+
+  /**
+   * Register a callback on whether the analog input is initialized.
+   *
+   * @param callback the callback that will be called whenever the analog input is initialized
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelInitializedCallback);
+  }
+
+  /**
+   * Check if this analog input has been initialized.
+   *
+   * @return true if initialized
+   */
+  public boolean getInitialized() {
+    return AnalogInDataJNI.getInitialized(m_index);
+  }
+
+  /**
+   * Change whether this analog input has been initialized.
+   *
+   * @param initialized the new value
+   */
+  public void setInitialized(boolean initialized) {
+    AnalogInDataJNI.setInitialized(m_index, initialized);
+  }
+
+  /**
+   * Register a callback on the number of average bits.
+   *
+   * @param callback the callback that will be called whenever the number of average bits is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerAverageBitsCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerAverageBitsCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAverageBitsCallback);
+  }
+
+  /**
+   * Get the number of average bits.
+   *
+   * @return the number of average bits
+   */
+  public int getAverageBits() {
+    return AnalogInDataJNI.getAverageBits(m_index);
+  }
+
+  /**
+   * Change the number of average bits.
+   *
+   * @param averageBits the new value
+   */
+  public void setAverageBits(int averageBits) {
+    AnalogInDataJNI.setAverageBits(m_index, averageBits);
+  }
+
+  /**
+   * Register a callback on the amount of oversampling bits.
+   *
+   * @param callback the callback that will be called whenever the oversampling bits are changed.
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerOversampleBitsCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerOversampleBitsCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelOversampleBitsCallback);
+  }
+
+  /**
+   * Get the amount of oversampling bits.
+   *
+   * @return the amount of oversampling bits
+   */
+  public int getOversampleBits() {
+    return AnalogInDataJNI.getOversampleBits(m_index);
+  }
+
+  /**
+   * Change the amount of oversampling bits.
+   *
+   * @param oversampleBits the new value
+   */
+  public void setOversampleBits(int oversampleBits) {
+    AnalogInDataJNI.setOversampleBits(m_index, oversampleBits);
+  }
+
+  /**
+   * Register a callback on the voltage.
+   *
+   * @param callback the callback that will be called whenever the voltage is changed.
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerVoltageCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelVoltageCallback);
+  }
+
+  /**
+   * Get the voltage.
+   *
+   * @return the voltage
+   */
+  public double getVoltage() {
+    return AnalogInDataJNI.getVoltage(m_index);
+  }
+
+  /**
+   * Change the voltage.
+   *
+   * @param voltage the new value
+   */
+  public void setVoltage(double voltage) {
+    AnalogInDataJNI.setVoltage(m_index, voltage);
+  }
+
+  /** Reset all simulation data for this object. */
+  public void resetData() {
+    AnalogInDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/BatterySim.java

@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+/** A utility class to simulate the robot battery. */
+public final class BatterySim {
+  private BatterySim() {
+    // Utility class
+  }
+
+  /**
+   * Calculate the loaded battery voltage. Use this with {@link RoboRioSim#setVInVoltage(double)} to
+   * set the simulated battery voltage, which can then be retrieved with the {@link
+   * edu.wpi.first.wpilibj.RobotController#getBatteryVoltage()} method.
+   *
+   * @param nominalVoltage The nominal battery voltage. Usually 12v.
+   * @param resistanceOhms The forward resistance of the battery. Most batteries are at or below 20
+   *     milliohms.
+   * @param currents The currents drawn from the battery.
+   * @return The battery's voltage under load.
+   */
+  public static double calculateLoadedBatteryVoltage(
+      double nominalVoltage, double resistanceOhms, double... currents) {
+    double retval = nominalVoltage;
+    for (var current : currents) {
+      retval -= current * resistanceOhms;
+    }
+    return Math.max(0.0, retval);
+  }
+
+  /**
+   * Calculate the loaded battery voltage. Use this with {@link RoboRioSim#setVInVoltage(double)} to
+   * set the simulated battery voltage, which can then be retrieved with the {@link
+   * edu.wpi.first.wpilibj.RobotController#getBatteryVoltage()} method. This function assumes a
+   * nominal voltage of 12v and a resistance of 20 milliohms (0.020 ohms)
+   *
+   * @param currents The currents drawn from the battery.
+   * @return The battery's voltage under load.
+   */
+  public static double calculateDefaultBatteryLoadedVoltage(double... currents) {
+    return calculateLoadedBatteryVoltage(12, 0.02, currents);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/CTREPCMSim.java

@@ -0,0 +1,157 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.CTREPCMDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.PneumaticsControlModule;
+import edu.wpi.first.wpilibj.SensorUtil;
+
+/** Class to control a simulated Pneumatic Control Module (PCM). */
+public class CTREPCMSim extends PneumaticsBaseSim {
+  /** Constructs for the default PCM. */
+  public CTREPCMSim() {
+    super(SensorUtil.getDefaultCTREPCMModule());
+  }
+
+  /**
+   * Constructs from a PCM module number (CAN ID).
+   *
+   * @param module module number
+   */
+  public CTREPCMSim(int module) {
+    super(module);
+  }
+
+  /**
+   * Constructs from a PneumaticsControlModule object.
+   *
+   * @param module PCM module to simulate
+   */
+  public CTREPCMSim(PneumaticsControlModule module) {
+    super(module);
+  }
+
+  /**
+   * Check whether the closed loop compressor control is active.
+   *
+   * @return true if active
+   */
+  public boolean getClosedLoopEnabled() {
+    return CTREPCMDataJNI.getClosedLoopEnabled(m_index);
+  }
+
+  /**
+   * Turn on/off the closed loop control of the compressor.
+   *
+   * @param closedLoopEnabled whether the control loop is active
+   */
+  public void setClosedLoopEnabled(boolean closedLoopEnabled) {
+    CTREPCMDataJNI.setClosedLoopEnabled(m_index, closedLoopEnabled);
+  }
+
+  /**
+   * Register a callback to be run whenever the closed loop state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerClosedLoopEnabledCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = CTREPCMDataJNI.registerClosedLoopEnabledCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, CTREPCMDataJNI::cancelClosedLoopEnabledCallback);
+  }
+
+  @Override
+  public boolean getInitialized() {
+    return CTREPCMDataJNI.getInitialized(m_index);
+  }
+
+  @Override
+  public void setInitialized(boolean initialized) {
+    CTREPCMDataJNI.setInitialized(m_index, initialized);
+  }
+
+  @Override
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = CTREPCMDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, CTREPCMDataJNI::cancelInitializedCallback);
+  }
+
+  @Override
+  public boolean getCompressorOn() {
+    return CTREPCMDataJNI.getCompressorOn(m_index);
+  }
+
+  @Override
+  public void setCompressorOn(boolean compressorOn) {
+    CTREPCMDataJNI.setCompressorOn(m_index, compressorOn);
+  }
+
+  @Override
+  public CallbackStore registerCompressorOnCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = CTREPCMDataJNI.registerCompressorOnCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, CTREPCMDataJNI::cancelCompressorOnCallback);
+  }
+
+  @Override
+  public boolean getSolenoidOutput(int channel) {
+    return CTREPCMDataJNI.getSolenoidOutput(m_index, channel);
+  }
+
+  @Override
+  public void setSolenoidOutput(int channel, boolean solenoidOutput) {
+    CTREPCMDataJNI.setSolenoidOutput(m_index, channel, solenoidOutput);
+  }
+
+  @Override
+  public CallbackStore registerSolenoidOutputCallback(
+      int channel, NotifyCallback callback, boolean initialNotify) {
+    int uid =
+        CTREPCMDataJNI.registerSolenoidOutputCallback(m_index, channel, callback, initialNotify);
+    return new CallbackStore(m_index, channel, uid, CTREPCMDataJNI::cancelSolenoidOutputCallback);
+  }
+
+  @Override
+  public boolean getPressureSwitch() {
+    return CTREPCMDataJNI.getPressureSwitch(m_index);
+  }
+
+  @Override
+  public void setPressureSwitch(boolean pressureSwitch) {
+    CTREPCMDataJNI.setPressureSwitch(m_index, pressureSwitch);
+  }
+
+  @Override
+  public CallbackStore registerPressureSwitchCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = CTREPCMDataJNI.registerPressureSwitchCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, CTREPCMDataJNI::cancelPressureSwitchCallback);
+  }
+
+  @Override
+  public double getCompressorCurrent() {
+    return CTREPCMDataJNI.getCompressorCurrent(m_index);
+  }
+
+  @Override
+  public void setCompressorCurrent(double compressorCurrent) {
+    CTREPCMDataJNI.setCompressorCurrent(m_index, compressorCurrent);
+  }
+
+  @Override
+  public CallbackStore registerCompressorCurrentCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = CTREPCMDataJNI.registerCompressorCurrentCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, CTREPCMDataJNI::cancelCompressorCurrentCallback);
+  }
+
+  @Override
+  public void resetData() {
+    CTREPCMDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/CallbackStore.java

@@ -0,0 +1,112 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+/** Manages simulation callbacks; each object is associated with a callback. */
+public class CallbackStore implements AutoCloseable {
+  /** <b>Note: This interface is for simulation classes only. It should not be used by teams!</b> */
+  @SuppressWarnings("PMD.ImplicitFunctionalInterface")
+  interface CancelCallbackFunc {
+    void cancel(int index, int uid);
+  }
+
+  /** <b>Note: This interface is for simulation classes only. It should not be used by teams!</b> */
+  @SuppressWarnings("PMD.ImplicitFunctionalInterface")
+  interface CancelCallbackChannelFunc {
+    void cancel(int index, int channel, int uid);
+  }
+
+  /** <b>Note: This interface is for simulation classes only. It should not be used by teams!</b> */
+  @SuppressWarnings("PMD.ImplicitFunctionalInterface")
+  interface CancelCallbackNoIndexFunc {
+    void cancel(int uid);
+  }
+
+  /**
+   * Constructs an empty CallbackStore. This constructor is to allow 3rd party sim providers (eg
+   * vendors) to subclass this class (without needing provide dummy constructing parameters) so that
+   * the register methods of their sim classes can return CallbackStores like the builtin sims.
+   * <b>Note: It should not be called by teams that are just using sims!</b>
+   */
+  protected CallbackStore() {
+    this.m_cancelType = -1;
+    this.m_index = -1;
+    this.m_uid = -1;
+    this.m_cancelCallback = null;
+    this.m_cancelCallbackChannel = null;
+  }
+
+  /**
+   * <b>Note: This constructor is for simulation classes only. It should not be called by teams!</b>
+   *
+   * @param index TODO
+   * @param uid TODO
+   * @param ccf TODO
+   */
+  public CallbackStore(int index, int uid, CancelCallbackFunc ccf) {
+    this.m_cancelType = kNormalCancel;
+    this.m_index = index;
+    this.m_uid = uid;
+    this.m_cancelCallback = ccf;
+  }
+
+  /**
+   * <b>Note: This constructor is for simulation classes only. It should not be called by teams!</b>
+   *
+   * @param index TODO
+   * @param channel TODO
+   * @param uid TODO
+   * @param ccf TODO
+   */
+  public CallbackStore(int index, int channel, int uid, CancelCallbackChannelFunc ccf) {
+    this.m_cancelType = kChannelCancel;
+    this.m_index = index;
+    this.m_uid = uid;
+    this.m_channel = channel;
+    this.m_cancelCallbackChannel = ccf;
+  }
+
+  /**
+   * <b>Note: This constructor is for simulation classes only. It should not be called by teams!</b>
+   *
+   * @param uid TODO
+   * @param ccf TODO
+   */
+  public CallbackStore(int uid, CancelCallbackNoIndexFunc ccf) {
+    this.m_cancelType = kNoIndexCancel;
+    this.m_uid = uid;
+    this.m_cancelCallbackNoIndex = ccf;
+  }
+
+  private int m_index;
+  private int m_channel;
+  private final int m_uid;
+  private CancelCallbackFunc m_cancelCallback;
+  private CancelCallbackChannelFunc m_cancelCallbackChannel;
+  private CancelCallbackNoIndexFunc m_cancelCallbackNoIndex;
+  private static final int kAlreadyCancelled = -1;
+  private static final int kNormalCancel = 0;
+  private static final int kChannelCancel = 1;
+  private static final int kNoIndexCancel = 2;
+  private int m_cancelType;
+
+  /** Cancel the callback associated with this object. */
+  @Override
+  public void close() {
+    switch (m_cancelType) {
+      case kAlreadyCancelled -> {
+        // Already cancelled so do nothing so that close() is idempotent.
+        return;
+      }
+      case kNormalCancel -> m_cancelCallback.cancel(m_index, m_uid);
+      case kChannelCancel -> m_cancelCallbackChannel.cancel(m_index, m_channel, m_uid);
+      case kNoIndexCancel -> m_cancelCallbackNoIndex.cancel(m_uid);
+      default -> {
+        assert false;
+      }
+    }
+    m_cancelType = kAlreadyCancelled;
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/DCMotorSim.java

@@ -0,0 +1,186 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.RobotController;
+
+/** Represents a simulated DC motor mechanism. */
+public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
+  // Gearbox for the DC motor.
+  private final DCMotor m_gearbox;
+
+  // The gearing from the motors to the output.
+  private final double m_gearing;
+
+  // The moment of inertia for the DC motor mechanism in kg-m².
+  private final double m_j;
+
+  /**
+   * Creates a simulated DC motor mechanism.
+   *
+   * @param plant The linear system representing the DC motor. This system can be created with
+   *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(DCMotor, double,
+   *     double)} or {@link
+   *     edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)}. If
+   *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)}
+   *     is used, the distance unit must be radians.
+   * @param gearbox The type of and number of motors in the DC motor gearbox.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 2 elements. The first element is for position. The
+   *     second element is for velocity.
+   */
+  public DCMotorSim(LinearSystem<N2, N1, N2> plant, DCMotor gearbox, double... measurementStdDevs) {
+    super(plant, measurementStdDevs);
+    m_gearbox = gearbox;
+
+    // By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf,
+    // the DC motor state-space model is:
+    //
+    //   dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u
+    //   A = -G²Kₜ/(KᵥRJ)
+    //   B = GKₜ/(RJ)
+    //
+    // Solve for G.
+    //
+    //   A/B = -G/Kᵥ
+    //   G = -KᵥA/B
+    //
+    // Solve for J.
+    //
+    //   B = GKₜ/(RJ)
+    //   J = GKₜ/(RB)
+    m_gearing = -gearbox.Kv * plant.getA(1, 1) / plant.getB(1, 0);
+    m_j = m_gearing * gearbox.Kt / (gearbox.R * plant.getB(1, 0));
+  }
+
+  /**
+   * Sets the state of the DC motor.
+   *
+   * @param angularPosition The new position in radians.
+   * @param angularVelocity The new velocity in radians per second.
+   */
+  public void setState(double angularPosition, double angularVelocity) {
+    setState(VecBuilder.fill(angularPosition, angularVelocity));
+  }
+
+  /**
+   * Sets the DC motor's angular position.
+   *
+   * @param angularPosition The new position in radians.
+   */
+  public void setAngle(double angularPosition) {
+    setState(angularPosition, getAngularVelocity());
+  }
+
+  /**
+   * Sets the DC motor's angular velocity.
+   *
+   * @param angularVelocity The new velocity in radians per second.
+   */
+  public void setAngularVelocity(double angularVelocity) {
+    setState(getAngularPosition(), angularVelocity);
+  }
+
+  /**
+   * Returns the gear ratio of the DC motor.
+   *
+   * @return the DC motor's gear ratio.
+   */
+  public double getGearing() {
+    return m_gearing;
+  }
+
+  /**
+   * Returns the moment of inertia of the DC motor.
+   *
+   * @return The DC motor's moment of inertia in kg-m².
+   */
+  public double getJ() {
+    return m_j;
+  }
+
+  /**
+   * Returns the gearbox for the DC motor.
+   *
+   * @return The DC motor's gearbox.
+   */
+  public DCMotor getGearbox() {
+    return m_gearbox;
+  }
+
+  /**
+   * Returns the DC motor's position.
+   *
+   * @return The DC motor's position in radians.
+   */
+  public double getAngularPosition() {
+    return getOutput(0);
+  }
+
+  /**
+   * Returns the DC motor's velocity.
+   *
+   * @return The DC motor's velocity in radians per second.
+   */
+  public double getAngularVelocity() {
+    return getOutput(1);
+  }
+
+  /**
+   * Returns the DC motor's acceleration.
+   *
+   * @return The DC motor's acceleration in rad/s².
+   */
+  public double getAngularAcceleration() {
+    var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
+    return acceleration.get(1, 0);
+  }
+
+  /**
+   * Returns the DC motor's torque in.
+   *
+   * @return The DC motor's torque in Newton-meters.
+   */
+  public double getTorque() {
+    return getAngularAcceleration() * m_j;
+  }
+
+  /**
+   * Returns the DC motor's current draw.
+   *
+   * @return The DC motor's current draw in amps.
+   */
+  public double getCurrentDraw() {
+    // I = V / R - omega / (Kv * R)
+    // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
+    // 2x faster than the output
+    return m_gearbox.getCurrent(m_x.get(1, 0) * m_gearing, m_u.get(0, 0))
+        * Math.signum(m_u.get(0, 0));
+  }
+
+  /**
+   * Gets the input voltage for the DC motor.
+   *
+   * @return The DC motor's input voltage.
+   */
+  public double getInputVoltage() {
+    return getInput(0);
+  }
+
+  /**
+   * Sets the input voltage for the DC motor.
+   *
+   * @param volts The input voltage.
+   */
+  public void setInputVoltage(double volts) {
+    setInput(volts);
+    clampInput(RobotController.getBatteryVoltage());
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/DIOSim.java

@@ -0,0 +1,197 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.DIODataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DigitalOutput;
+
+/** Class to control a simulated digital input or output. */
+public class DIOSim {
+  private final int m_index;
+
+  /**
+   * Constructs from a DigitalInput object.
+   *
+   * @param input DigitalInput to simulate
+   */
+  public DIOSim(DigitalInput input) {
+    m_index = input.getChannel();
+  }
+
+  /**
+   * Constructs from a DigitalOutput object.
+   *
+   * @param output DigitalOutput to simulate
+   */
+  public DIOSim(DigitalOutput output) {
+    m_index = output.getChannel();
+  }
+
+  /**
+   * Constructs from a digital I/O channel number.
+   *
+   * @param channel Channel number
+   */
+  public DIOSim(int channel) {
+    m_index = channel;
+  }
+
+  /**
+   * Register a callback to be run when this DIO is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DIODataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DIODataJNI::cancelInitializedCallback);
+  }
+
+  /**
+   * Check whether this DIO has been initialized.
+   *
+   * @return true if initialized
+   */
+  public boolean getInitialized() {
+    return DIODataJNI.getInitialized(m_index);
+  }
+
+  /**
+   * Define whether this DIO has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
+  public void setInitialized(boolean initialized) {
+    DIODataJNI.setInitialized(m_index, initialized);
+  }
+
+  /**
+   * Register a callback to be run whenever the DIO value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerValueCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DIODataJNI.registerValueCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DIODataJNI::cancelValueCallback);
+  }
+
+  /**
+   * Read the value of the DIO port.
+   *
+   * @return the DIO value
+   */
+  public boolean getValue() {
+    return DIODataJNI.getValue(m_index);
+  }
+
+  /**
+   * Change the DIO value.
+   *
+   * @param value the new value
+   */
+  public void setValue(boolean value) {
+    DIODataJNI.setValue(m_index, value);
+  }
+
+  /**
+   * Register a callback to be run whenever the pulse length changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerPulseLengthCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DIODataJNI.registerPulseLengthCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DIODataJNI::cancelPulseLengthCallback);
+  }
+
+  /**
+   * Read the pulse length.
+   *
+   * @return the pulse length of this DIO port
+   */
+  public double getPulseLength() {
+    return DIODataJNI.getPulseLength(m_index);
+  }
+
+  /**
+   * Change the pulse length of this DIO port.
+   *
+   * @param pulseLength the new pulse length
+   */
+  public void setPulseLength(double pulseLength) {
+    DIODataJNI.setPulseLength(m_index, pulseLength);
+  }
+
+  /**
+   * Register a callback to be run whenever this DIO changes to be an input.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerIsInputCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DIODataJNI.registerIsInputCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DIODataJNI::cancelIsInputCallback);
+  }
+
+  /**
+   * Check whether this DIO port is currently an Input.
+   *
+   * @return true if Input
+   */
+  public boolean getIsInput() {
+    return DIODataJNI.getIsInput(m_index);
+  }
+
+  /**
+   * Define whether this DIO port is an Input.
+   *
+   * @param isInput whether this DIO should be an Input
+   */
+  public void setIsInput(boolean isInput) {
+    DIODataJNI.setIsInput(m_index, isInput);
+  }
+
+  /**
+   * Register a callback to be run whenever the filter index changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerFilterIndexCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DIODataJNI.registerFilterIndexCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DIODataJNI::cancelFilterIndexCallback);
+  }
+
+  /**
+   * Read the filter index.
+   *
+   * @return the filter index of this DIO port
+   */
+  public int getFilterIndex() {
+    return DIODataJNI.getFilterIndex(m_index);
+  }
+
+  /**
+   * Change the filter index of this DIO port.
+   *
+   * @param filterIndex the new filter index
+   */
+  public void setFilterIndex(int filterIndex) {
+    DIODataJNI.setFilterIndex(m_index, filterIndex);
+  }
+
+  /** Reset all simulation data of this object. */
+  public void resetData() {
+    DIODataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java

@@ -0,0 +1,493 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N7;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotController;
+
+/**
+ * This class simulates the state of the drivetrain. In simulationPeriodic, users should first set
+ * inputs from motors with {@link #setInputs(double, double)}, call {@link #update(double)} to
+ * update the simulation, and set estimated encoder and gyro positions, as well as estimated
+ * odometry pose. Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize
+ * their robot on the Sim GUI's field.
+ *
+ * <p>Our state-space system is:
+ *
+ * <p>x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r]]ᵀ in the field coordinate system (dist_* are
+ * wheel distances.)
+ *
+ * <p>u = [[voltage_l, voltage_r]]ᵀ This is typically the control input of the last timestep from a
+ * LTVDiffDriveController.
+ *
+ * <p>y = x
+ */
+public class DifferentialDrivetrainSim {
+  private final DCMotor m_motor;
+  private final double m_originalGearing;
+  private final Matrix<N7, N1> m_measurementStdDevs;
+  private double m_currentGearing;
+  private final double m_wheelRadius;
+
+  private Matrix<N2, N1> m_u;
+  private Matrix<N7, N1> m_x;
+  private Matrix<N7, N1> m_y;
+
+  private final double m_rb;
+  private final LinearSystem<N2, N2, N2> m_plant;
+
+  /**
+   * Creates a simulated differential drivetrain.
+   *
+   * @param driveMotor A {@link DCMotor} representing the left side of the drivetrain.
+   * @param gearing The gearing ratio between motor and wheel, as output over input. This must be
+   *     the same ratio as the ratio used to identify or create the drivetrainPlant.
+   * @param j The moment of inertia of the drivetrain about its center in kg-m².
+   * @param mass The mass of the drivebase in kg.
+   * @param wheelRadius The radius of the wheels on the drivetrain in meters.
+   * @param trackwidth The robot's trackwidth, or distance between left and right wheels, in meters.
+   * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
+   *     left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
+   *     desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
+   *     m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
+   *     point.
+   */
+  public DifferentialDrivetrainSim(
+      DCMotor driveMotor,
+      double gearing,
+      double j,
+      double mass,
+      double wheelRadius,
+      double trackwidth,
+      Matrix<N7, N1> measurementStdDevs) {
+    this(
+        LinearSystemId.createDrivetrainVelocitySystem(
+            driveMotor, mass, wheelRadius, trackwidth / 2.0, j, gearing),
+        driveMotor,
+        gearing,
+        trackwidth,
+        wheelRadius,
+        measurementStdDevs);
+  }
+
+  /**
+   * Creates a simulated differential drivetrain.
+   *
+   * @param plant The {@link LinearSystem} representing the robot's drivetrain. This system can be
+   *     created with {@link
+   *     edu.wpi.first.math.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor,
+   *     double, double, double, double, double)} or {@link
+   *     edu.wpi.first.math.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double,
+   *     double, double)}.
+   * @param driveMotor A {@link DCMotor} representing the drivetrain.
+   * @param gearing The gearingRatio ratio of the robot, as output over input. This must be the same
+   *     ratio as the ratio used to identify or create the drivetrainPlant.
+   * @param trackwidth The distance between the two sides of the drivetrain in meters. Can be found
+   *     with SysId.
+   * @param wheelRadius The radius of the wheels on the drivetrain, in meters.
+   * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
+   *     left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
+   *     desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
+   *     m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
+   *     point.
+   */
+  public DifferentialDrivetrainSim(
+      LinearSystem<N2, N2, N2> plant,
+      DCMotor driveMotor,
+      double gearing,
+      double trackwidth,
+      double wheelRadius,
+      Matrix<N7, N1> measurementStdDevs) {
+    this.m_plant = plant;
+    this.m_rb = trackwidth / 2.0;
+    this.m_motor = driveMotor;
+    this.m_originalGearing = gearing;
+    this.m_measurementStdDevs = measurementStdDevs;
+    m_wheelRadius = wheelRadius;
+    m_currentGearing = m_originalGearing;
+
+    m_x = new Matrix<>(Nat.N7(), Nat.N1());
+    m_u = VecBuilder.fill(0, 0);
+    m_y = new Matrix<>(Nat.N7(), Nat.N1());
+  }
+
+  /**
+   * Sets the applied voltage to the drivetrain. Note that positive voltage must make that side of
+   * the drivetrain travel forward (+X).
+   *
+   * @param leftVoltageVolts The left voltage.
+   * @param rightVoltageVolts The right voltage.
+   */
+  public void setInputs(double leftVoltageVolts, double rightVoltageVolts) {
+    m_u = clampInput(VecBuilder.fill(leftVoltageVolts, rightVoltageVolts));
+  }
+
+  /**
+   * Update the drivetrain states with the current time difference.
+   *
+   * @param dt the time difference in seconds
+   */
+  public void update(double dt) {
+    m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dt);
+    m_y = m_x;
+    if (m_measurementStdDevs != null) {
+      m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
+    }
+  }
+
+  /** Returns the full simulated state of the drivetrain. */
+  Matrix<N7, N1> getState() {
+    return m_x;
+  }
+
+  /**
+   * Get one of the drivetrain states.
+   *
+   * @param state the state to get
+   * @return the state
+   */
+  double getState(State state) {
+    return m_x.get(state.value, 0);
+  }
+
+  private double getOutput(State output) {
+    return m_y.get(output.value, 0);
+  }
+
+  /**
+   * Returns the direction the robot is pointing.
+   *
+   * <p>Note that this angle is counterclockwise-positive, while most gyros are clockwise positive.
+   *
+   * @return The direction the robot is pointing.
+   */
+  public Rotation2d getHeading() {
+    return new Rotation2d(getOutput(State.kHeading));
+  }
+
+  /**
+   * Returns the current pose.
+   *
+   * @return The current pose.
+   */
+  public Pose2d getPose() {
+    return new Pose2d(getOutput(State.kX), getOutput(State.kY), getHeading());
+  }
+
+  /**
+   * Get the right encoder position.
+   *
+   * @return The encoder position in meters.
+   */
+  public double getRightPosition() {
+    return getOutput(State.kRightPosition);
+  }
+
+  /**
+   * Get the right encoder velocity.
+   *
+   * @return The encoder velocity in meters per second.
+   */
+  public double getRightVelocity() {
+    return getOutput(State.kRightVelocity);
+  }
+
+  /**
+   * Get the left encoder position.
+   *
+   * @return The encoder position in meters.
+   */
+  public double getLeftPosition() {
+    return getOutput(State.kLeftPosition);
+  }
+
+  /**
+   * Get the left encoder velocity.
+   *
+   * @return The encoder velocity in meters per second.
+   */
+  public double getLeftVelocity() {
+    return getOutput(State.kLeftVelocity);
+  }
+
+  /**
+   * Get the current draw of the left side of the drivetrain.
+   *
+   * @return the drivetrain's left side current draw, in amps
+   */
+  public double getLeftCurrentDraw() {
+    return m_motor.getCurrent(
+            getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadius, m_u.get(0, 0))
+        * Math.signum(m_u.get(0, 0));
+  }
+
+  /**
+   * Get the current draw of the right side of the drivetrain.
+   *
+   * @return the drivetrain's right side current draw, in amps
+   */
+  public double getRightCurrentDraw() {
+    return m_motor.getCurrent(
+            getState(State.kRightVelocity) * m_currentGearing / m_wheelRadius, m_u.get(1, 0))
+        * Math.signum(m_u.get(1, 0));
+  }
+
+  /**
+   * Get the current draw of the drivetrain.
+   *
+   * @return the current draw, in amps
+   */
+  public double getCurrentDraw() {
+    return getLeftCurrentDraw() + getRightCurrentDraw();
+  }
+
+  /**
+   * Get the drivetrain gearing.
+   *
+   * @return the gearing ration
+   */
+  public double getCurrentGearing() {
+    return m_currentGearing;
+  }
+
+  /**
+   * Sets the gearing reduction on the drivetrain. This is commonly used for shifting drivetrains.
+   *
+   * @param newGearRatio The new gear ratio, as output over input.
+   */
+  public void setCurrentGearing(double newGearRatio) {
+    this.m_currentGearing = newGearRatio;
+  }
+
+  /**
+   * Sets the system state.
+   *
+   * @param state The state.
+   */
+  public void setState(Matrix<N7, N1> state) {
+    m_x = state;
+  }
+
+  /**
+   * Sets the system pose.
+   *
+   * @param pose The pose.
+   */
+  public void setPose(Pose2d pose) {
+    m_x.set(State.kX.value, 0, pose.getX());
+    m_x.set(State.kY.value, 0, pose.getY());
+    m_x.set(State.kHeading.value, 0, pose.getRotation().getRadians());
+    m_x.set(State.kLeftPosition.value, 0, 0);
+    m_x.set(State.kRightPosition.value, 0, 0);
+  }
+
+  /**
+   * The differential drive dynamics function.
+   *
+   * @param x The state.
+   * @param u The input.
+   * @return The state derivative with respect to time.
+   */
+  protected Matrix<N7, N1> getDynamics(Matrix<N7, N1> x, Matrix<N2, N1> u) {
+    // Because G can be factored out of B, we can divide by the old ratio and multiply
+    // by the new ratio to get a new drivetrain model.
+    var B = new Matrix<>(Nat.N4(), Nat.N2());
+    B.assignBlock(0, 0, m_plant.getB().times(this.m_currentGearing / this.m_originalGearing));
+
+    // Because G² can be factored out of A, we can divide by the old ratio squared and multiply
+    // by the new ratio squared to get a new drivetrain model.
+    var A = new Matrix<>(Nat.N4(), Nat.N4());
+    A.assignBlock(
+        0,
+        0,
+        m_plant
+            .getA()
+            .times(
+                (this.m_currentGearing * this.m_currentGearing)
+                    / (this.m_originalGearing * this.m_originalGearing)));
+
+    A.assignBlock(2, 0, Matrix.eye(Nat.N2()));
+
+    var v = (x.get(State.kLeftVelocity.value, 0) + x.get(State.kRightVelocity.value, 0)) / 2.0;
+
+    var xdot = new Matrix<>(Nat.N7(), Nat.N1());
+    xdot.set(0, 0, v * Math.cos(x.get(State.kHeading.value, 0)));
+    xdot.set(1, 0, v * Math.sin(x.get(State.kHeading.value, 0)));
+    xdot.set(
+        2,
+        0,
+        (x.get(State.kRightVelocity.value, 0) - x.get(State.kLeftVelocity.value, 0))
+            / (2.0 * m_rb));
+    xdot.assignBlock(3, 0, A.times(x.block(Nat.N4(), Nat.N1(), 3, 0)).plus(B.times(u)));
+
+    return xdot;
+  }
+
+  /**
+   * Clamp the input vector such that no element exceeds the battery voltage. If any does, the
+   * relative magnitudes of the input will be maintained.
+   *
+   * @param u The input vector.
+   * @return The normalized input.
+   */
+  protected Matrix<N2, N1> clampInput(Matrix<N2, N1> u) {
+    return StateSpaceUtil.desaturateInputVector(u, RobotController.getBatteryVoltage());
+  }
+
+  /** Represents the different states of the drivetrain. */
+  enum State {
+    kX(0),
+    kY(1),
+    kHeading(2),
+    kLeftVelocity(3),
+    kRightVelocity(4),
+    kLeftPosition(5),
+    kRightPosition(6);
+
+    public final int value;
+
+    State(int i) {
+      this.value = i;
+    }
+  }
+
+  /**
+   * Represents a gearing option of the Toughbox mini. 12.75:1 -- 14:50 and 14:50 10.71:1 -- 14:50
+   * and 16:48 8.45:1 -- 14:50 and 19:45 7.31:1 -- 14:50 and 21:43 5.95:1 -- 14:50 and 24:40
+   */
+  public enum KitbotGearing {
+    /** Gear ratio of 12.75:1. */
+    k12p75(12.75),
+    /** Gear ratio of 10.71:1. */
+    k10p71(10.71),
+    /** Gear ratio of 8.45:1. */
+    k8p45(8.45),
+    /** Gear ratio of 7.31:1. */
+    k7p31(7.31),
+    /** Gear ratio of 5.95:1. */
+    k5p95(5.95);
+
+    /** KitbotGearing value. */
+    public final double value;
+
+    KitbotGearing(double i) {
+      this.value = i;
+    }
+  }
+
+  /** Represents common motor layouts of the kit drivetrain. */
+  public enum KitbotMotor {
+    /** One CIM motor per drive side. */
+    kSingleCIMPerSide(DCMotor.getCIM(1)),
+    /** Two CIM motors per drive side. */
+    kDualCIMPerSide(DCMotor.getCIM(2)),
+    /** One Mini CIM motor per drive side. */
+    kSingleMiniCIMPerSide(DCMotor.getMiniCIM(1)),
+    /** Two Mini CIM motors per drive side. */
+    kDualMiniCIMPerSide(DCMotor.getMiniCIM(2)),
+    /** One Falcon 500 motor per drive side. */
+    kSingleFalcon500PerSide(DCMotor.getFalcon500(1)),
+    /** Two Falcon 500 motors per drive side. */
+    kDoubleFalcon500PerSide(DCMotor.getFalcon500(2)),
+    /** One NEO motor per drive side. */
+    kSingleNEOPerSide(DCMotor.getNEO(1)),
+    /** Two NEO motors per drive side. */
+    kDoubleNEOPerSide(DCMotor.getNEO(2));
+
+    /** KitbotMotor value. */
+    public final DCMotor value;
+
+    KitbotMotor(DCMotor i) {
+      this.value = i;
+    }
+  }
+
+  /** Represents common wheel sizes of the kit drivetrain. */
+  public enum KitbotWheelSize {
+    /** Six inch diameter wheels. */
+    kSixInch(Units.inchesToMeters(6)),
+    /** Eight inch diameter wheels. */
+    kEightInch(Units.inchesToMeters(8)),
+    /** Ten inch diameter wheels. */
+    kTenInch(Units.inchesToMeters(10));
+
+    /** KitbotWheelSize value. */
+    public final double value;
+
+    KitbotWheelSize(double i) {
+      this.value = i;
+    }
+  }
+
+  /**
+   * Create a sim for the standard FRC kitbot.
+   *
+   * @param motor The motors installed in the bot.
+   * @param gearing The gearing reduction used.
+   * @param wheelSize The wheel size.
+   * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
+   *     left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
+   *     desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
+   *     m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
+   *     point.
+   * @return A sim for the standard FRC kitbot.
+   */
+  public static DifferentialDrivetrainSim createKitbotSim(
+      KitbotMotor motor,
+      KitbotGearing gearing,
+      KitbotWheelSize wheelSize,
+      Matrix<N7, N1> measurementStdDevs) {
+    // MOI estimation -- note that I = mr² for point masses
+    var batteryMoi = 12.5 / 2.2 * Math.pow(Units.inchesToMeters(10), 2);
+    var gearboxMoi =
+        (2.8 /* CIM motor */ * 2 / 2.2 + 2.0 /* Toughbox Mini- ish */)
+            * Math.pow(Units.inchesToMeters(26.0 / 2.0), 2);
+
+    return createKitbotSim(motor, gearing, wheelSize, batteryMoi + gearboxMoi, measurementStdDevs);
+  }
+
+  /**
+   * Create a sim for the standard FRC kitbot.
+   *
+   * @param motor The motors installed in the bot.
+   * @param gearing The gearing reduction used.
+   * @param wheelSize The wheel size.
+   * @param j The moment of inertia of the drivebase in kg-m². This can be calculated using SysId.
+   * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
+   *     left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
+   *     desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
+   *     m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
+   *     point.
+   * @return A sim for the standard FRC kitbot.
+   */
+  public static DifferentialDrivetrainSim createKitbotSim(
+      KitbotMotor motor,
+      KitbotGearing gearing,
+      KitbotWheelSize wheelSize,
+      double j,
+      Matrix<N7, N1> measurementStdDevs) {
+    return new DifferentialDrivetrainSim(
+        motor.value,
+        gearing.value,
+        j,
+        Units.lbsToKilograms(60),
+        wheelSize.value / 2.0,
+        Units.inchesToMeters(26),
+        measurementStdDevs);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/DigitalPWMSim.java

@@ -0,0 +1,154 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.DigitalPWMDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import java.util.NoSuchElementException;
+
+/**
+ * Class to control a simulated digital PWM output.
+ *
+ * <p>This is for duty cycle PWM outputs on a DigitalOutput, not for the servo style PWM outputs on
+ * a PWM channel.
+ */
+public class DigitalPWMSim {
+  private final int m_index;
+
+  /**
+   * Constructs from a DigitalOutput object.
+   *
+   * @param digitalOutput DigitalOutput to simulate
+   */
+  public DigitalPWMSim(DigitalOutput digitalOutput) {
+    m_index = digitalOutput.getChannel();
+  }
+
+  private DigitalPWMSim(int index) {
+    m_index = index;
+  }
+
+  /**
+   * Creates an DigitalPWMSim for a digital I/O channel.
+   *
+   * @param channel DIO channel
+   * @return Simulated object
+   * @throws NoSuchElementException if no Digital PWM is configured for that channel
+   */
+  public static DigitalPWMSim createForChannel(int channel) {
+    int index = DigitalPWMDataJNI.findForChannel(channel);
+    if (index < 0) {
+      throw new NoSuchElementException("no digital PWM found for channel " + channel);
+    }
+    return new DigitalPWMSim(index);
+  }
+
+  /**
+   * Creates an DigitalPWMSim for a simulated index. The index is incremented for each simulated
+   * DigitalPWM.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  public static DigitalPWMSim createForIndex(int index) {
+    return new DigitalPWMSim(index);
+  }
+
+  /**
+   * Register a callback to be run when this PWM output is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DigitalPWMDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelInitializedCallback);
+  }
+
+  /**
+   * Check whether this PWM output has been initialized.
+   *
+   * @return true if initialized
+   */
+  public boolean getInitialized() {
+    return DigitalPWMDataJNI.getInitialized(m_index);
+  }
+
+  /**
+   * Define whether this PWM output has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
+  public void setInitialized(boolean initialized) {
+    DigitalPWMDataJNI.setInitialized(m_index, initialized);
+  }
+
+  /**
+   * Register a callback to be run whenever the duty cycle value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerDutyCycleCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DigitalPWMDataJNI.registerDutyCycleCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelDutyCycleCallback);
+  }
+
+  /**
+   * Read the duty cycle value.
+   *
+   * @return the duty cycle value of this PWM output
+   */
+  public double getDutyCycle() {
+    return DigitalPWMDataJNI.getDutyCycle(m_index);
+  }
+
+  /**
+   * Set the duty cycle value of this PWM output.
+   *
+   * @param dutyCycle the new value
+   */
+  public void setDutyCycle(double dutyCycle) {
+    DigitalPWMDataJNI.setDutyCycle(m_index, dutyCycle);
+  }
+
+  /**
+   * Register a callback to be run whenever the pin changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerPinCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DigitalPWMDataJNI.registerPinCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelPinCallback);
+  }
+
+  /**
+   * Check the pin number.
+   *
+   * @return the pin number
+   */
+  public int getPin() {
+    return DigitalPWMDataJNI.getPin(m_index);
+  }
+
+  /**
+   * Change the pin number.
+   *
+   * @param pin the new pin number
+   */
+  public void setPin(int pin) {
+    DigitalPWMDataJNI.setPin(m_index, pin);
+  }
+
+  /** Reset all simulation data. */
+  public void resetData() {
+    DigitalPWMDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/DoubleSolenoidSim.java

@@ -0,0 +1,88 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsBase;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+
+/** Class to control a simulated {@link edu.wpi.first.wpilibj.DoubleSolenoid}. */
+public class DoubleSolenoidSim {
+  private final PneumaticsBaseSim m_module;
+  private final int m_fwd;
+  private final int m_rev;
+
+  /**
+   * Constructs for a solenoid on the given pneumatics module.
+   *
+   * @param moduleSim the PCM the solenoid is connected to.
+   * @param fwd the forward solenoid channel.
+   * @param rev the reverse solenoid channel.
+   */
+  public DoubleSolenoidSim(PneumaticsBaseSim moduleSim, int fwd, int rev) {
+    m_module = moduleSim;
+    m_fwd = fwd;
+    m_rev = rev;
+  }
+
+  /**
+   * Constructs for a solenoid on a pneumatics module of the given type and ID.
+   *
+   * @param module the CAN ID of the pneumatics module the solenoid is connected to.
+   * @param moduleType the module type (PH or PCM)
+   * @param fwd the forward solenoid channel.
+   * @param rev the reverse solenoid channel.
+   */
+  public DoubleSolenoidSim(int module, PneumaticsModuleType moduleType, int fwd, int rev) {
+    this(PneumaticsBaseSim.getForType(module, moduleType), fwd, rev);
+  }
+
+  /**
+   * Constructs for a solenoid on a pneumatics module of the given type and default ID.
+   *
+   * @param moduleType the module type (PH or PCM)
+   * @param fwd the forward solenoid channel.
+   * @param rev the reverse solenoid channel.
+   */
+  public DoubleSolenoidSim(PneumaticsModuleType moduleType, int fwd, int rev) {
+    this(PneumaticsBase.getDefaultForType(moduleType), moduleType, fwd, rev);
+  }
+
+  /**
+   * Check the value of the double solenoid output.
+   *
+   * @return the output value of the double solenoid.
+   */
+  public DoubleSolenoid.Value get() {
+    boolean fwdState = m_module.getSolenoidOutput(m_fwd);
+    boolean revState = m_module.getSolenoidOutput(m_rev);
+    if (fwdState && !revState) {
+      return DoubleSolenoid.Value.kForward;
+    } else if (!fwdState && revState) {
+      return DoubleSolenoid.Value.kReverse;
+    } else {
+      return DoubleSolenoid.Value.kOff;
+    }
+  }
+
+  /**
+   * Set the value of the double solenoid output.
+   *
+   * @param value The value to set (Off, Forward, Reverse)
+   */
+  public void set(final DoubleSolenoid.Value value) {
+    m_module.setSolenoidOutput(m_fwd, value == DoubleSolenoid.Value.kForward);
+    m_module.setSolenoidOutput(m_rev, value == DoubleSolenoid.Value.kReverse);
+  }
+
+  /**
+   * Get the wrapped {@link PneumaticsBaseSim} object.
+   *
+   * @return the wrapped {@link PneumaticsBaseSim} object.
+   */
+  public PneumaticsBaseSim getModuleSim() {
+    return m_module;
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/DriverStationSim.java

@@ -0,0 +1,523 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.AllianceStationID;
+import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.hal.simulation.DriverStationDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.wpilibj.DriverStation;
+
+/** Class to control a simulated driver station. */
+public final class DriverStationSim {
+  private DriverStationSim() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Register a callback on whether the DS is enabled.
+   *
+   * @param callback the callback that will be called whenever the enabled state is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerEnabledCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerEnabledCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelEnabledCallback);
+  }
+
+  /**
+   * Check if the DS is enabled.
+   *
+   * @return true if enabled
+   */
+  public static boolean getEnabled() {
+    return DriverStationDataJNI.getEnabled();
+  }
+
+  /**
+   * Change whether the DS is enabled.
+   *
+   * @param enabled the new value
+   */
+  public static void setEnabled(boolean enabled) {
+    DriverStationDataJNI.setEnabled(enabled);
+  }
+
+  /**
+   * Register a callback on whether the DS is in autonomous mode.
+   *
+   * @param callback the callback that will be called on autonomous mode entrance/exit
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerAutonomousCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerAutonomousCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelAutonomousCallback);
+  }
+
+  /**
+   * Check if the DS is in autonomous.
+   *
+   * @return true if autonomous
+   */
+  public static boolean getAutonomous() {
+    return DriverStationDataJNI.getAutonomous();
+  }
+
+  /**
+   * Change whether the DS is in autonomous.
+   *
+   * @param autonomous the new value
+   */
+  public static void setAutonomous(boolean autonomous) {
+    DriverStationDataJNI.setAutonomous(autonomous);
+  }
+
+  /**
+   * Register a callback on whether the DS is in test mode.
+   *
+   * @param callback the callback that will be called whenever the test mode is entered or left
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerTestCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerTestCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelTestCallback);
+  }
+
+  /**
+   * Check if the DS is in test.
+   *
+   * @return true if test
+   */
+  public static boolean getTest() {
+    return DriverStationDataJNI.getTest();
+  }
+
+  /**
+   * Change whether the DS is in test.
+   *
+   * @param test the new value
+   */
+  public static void setTest(boolean test) {
+    DriverStationDataJNI.setTest(test);
+  }
+
+  /**
+   * Register a callback on the eStop state.
+   *
+   * @param callback the callback that will be called whenever the eStop state changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerEStopCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerEStopCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelEStopCallback);
+  }
+
+  /**
+   * Check if eStop has been activated.
+   *
+   * @return true if eStopped
+   */
+  public static boolean getEStop() {
+    return DriverStationDataJNI.getEStop();
+  }
+
+  /**
+   * Set whether eStop is active.
+   *
+   * @param eStop true to activate
+   */
+  public static void setEStop(boolean eStop) {
+    DriverStationDataJNI.setEStop(eStop);
+  }
+
+  /**
+   * Register a callback on whether the FMS is connected.
+   *
+   * @param callback the callback that will be called whenever the FMS connection changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerFmsAttachedCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerFmsAttachedCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelFmsAttachedCallback);
+  }
+
+  /**
+   * Check if the FMS is connected.
+   *
+   * @return true if FMS is connected
+   */
+  public static boolean getFmsAttached() {
+    return DriverStationDataJNI.getFmsAttached();
+  }
+
+  /**
+   * Change whether the FMS is connected.
+   *
+   * @param fmsAttached the new value
+   */
+  public static void setFmsAttached(boolean fmsAttached) {
+    DriverStationDataJNI.setFmsAttached(fmsAttached);
+  }
+
+  /**
+   * Register a callback on whether the DS is connected.
+   *
+   * @param callback the callback that will be called whenever the DS connection changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerDsAttachedCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerDsAttachedCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelDsAttachedCallback);
+  }
+
+  /**
+   * Check if the DS is attached.
+   *
+   * @return true if attached
+   */
+  public static boolean getDsAttached() {
+    return DriverStationDataJNI.getDsAttached();
+  }
+
+  /**
+   * Change whether the DS is attached.
+   *
+   * @param dsAttached the new value
+   */
+  public static void setDsAttached(boolean dsAttached) {
+    DriverStationDataJNI.setDsAttached(dsAttached);
+  }
+
+  /**
+   * Register a callback on the alliance station ID.
+   *
+   * @param callback the callback that will be called whenever the alliance station changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerAllianceStationIdCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerAllianceStationIdCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelAllianceStationIdCallback);
+  }
+
+  /**
+   * Get the alliance station ID (color + number).
+   *
+   * @return the alliance station color and number
+   */
+  public static AllianceStationID getAllianceStationId() {
+    return switch (DriverStationDataJNI.getAllianceStationId()) {
+      case DriverStationJNI.kUnknownAllianceStation -> AllianceStationID.Unknown;
+      case DriverStationJNI.kRed1AllianceStation -> AllianceStationID.Red1;
+      case DriverStationJNI.kRed2AllianceStation -> AllianceStationID.Red2;
+      case DriverStationJNI.kRed3AllianceStation -> AllianceStationID.Red3;
+      case DriverStationJNI.kBlue1AllianceStation -> AllianceStationID.Blue1;
+      case DriverStationJNI.kBlue2AllianceStation -> AllianceStationID.Blue2;
+      case DriverStationJNI.kBlue3AllianceStation -> AllianceStationID.Blue3;
+      default -> AllianceStationID.Unknown;
+    };
+  }
+
+  /**
+   * Change the alliance station.
+   *
+   * @param allianceStationId the new alliance station
+   */
+  public static void setAllianceStationId(AllianceStationID allianceStationId) {
+    int allianceStation =
+        switch (allianceStationId) {
+          case Unknown -> DriverStationJNI.kUnknownAllianceStation;
+          case Red1 -> DriverStationJNI.kRed1AllianceStation;
+          case Red2 -> DriverStationJNI.kRed2AllianceStation;
+          case Red3 -> DriverStationJNI.kRed3AllianceStation;
+          case Blue1 -> DriverStationJNI.kBlue1AllianceStation;
+          case Blue2 -> DriverStationJNI.kBlue2AllianceStation;
+          case Blue3 -> DriverStationJNI.kBlue3AllianceStation;
+        };
+    DriverStationDataJNI.setAllianceStationId(allianceStation);
+  }
+
+  /**
+   * Register a callback on match time.
+   *
+   * @param callback the callback that will be called whenever match time changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerMatchTimeCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerMatchTimeCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelMatchTimeCallback);
+  }
+
+  /**
+   * Get the current value of the match timer.
+   *
+   * @return the current match time
+   */
+  public static double getMatchTime() {
+    return DriverStationDataJNI.getMatchTime();
+  }
+
+  /**
+   * Sets the match timer.
+   *
+   * @param matchTime the new match time
+   */
+  public static void setMatchTime(double matchTime) {
+    DriverStationDataJNI.setMatchTime(matchTime);
+  }
+
+  /** Updates DriverStation data so that new values are visible to the user program. */
+  public static void notifyNewData() {
+    int handle = WPIUtilJNI.createEvent(false, false);
+    DriverStationJNI.provideNewDataEventHandle(handle);
+    DriverStationDataJNI.notifyNewData();
+    try {
+      WPIUtilJNI.waitForObject(handle);
+    } catch (InterruptedException e) {
+      e.printStackTrace();
+    }
+    DriverStationJNI.removeNewDataEventHandle(handle);
+    WPIUtilJNI.destroyEvent(handle);
+    DriverStation.refreshData();
+  }
+
+  /**
+   * Sets suppression of DriverStation.reportError and reportWarning messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  public static void setSendError(boolean shouldSend) {
+    DriverStationDataJNI.setSendError(shouldSend);
+  }
+
+  /**
+   * Sets suppression of DriverStation.sendConsoleLine messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  public static void setSendConsoleLine(boolean shouldSend) {
+    DriverStationDataJNI.setSendConsoleLine(shouldSend);
+  }
+
+  /**
+   * Gets the joystick outputs.
+   *
+   * @param stick The joystick number
+   * @return The joystick outputs
+   */
+  public static long getJoystickOutputs(int stick) {
+    return DriverStationDataJNI.getJoystickOutputs(stick);
+  }
+
+  /**
+   * Gets the joystick rumble.
+   *
+   * @param stick The joystick number
+   * @param rumbleNum Rumble to get (0=left, 1=right)
+   * @return The joystick rumble value
+   */
+  public static int getJoystickRumble(int stick, int rumbleNum) {
+    return DriverStationDataJNI.getJoystickRumble(stick, rumbleNum);
+  }
+
+  /**
+   * Sets the state of one joystick button. Button indexes begin at 0.
+   *
+   * @param stick The joystick number
+   * @param button The button index, beginning at 0
+   * @param state The state of the joystick button
+   */
+  public static void setJoystickButton(int stick, int button, boolean state) {
+    DriverStationDataJNI.setJoystickButton(stick, button, state);
+  }
+
+  /**
+   * Gets the value of the axis on a joystick.
+   *
+   * @param stick The joystick number
+   * @param axis The analog axis number
+   * @param value The value of the axis on the joystick
+   */
+  public static void setJoystickAxis(int stick, int axis, double value) {
+    DriverStationDataJNI.setJoystickAxis(stick, axis, value);
+  }
+
+  /**
+   * Gets the state of a POV on a joystick.
+   *
+   * @param stick The joystick number
+   * @param pov The POV number
+   * @param value the angle of the POV
+   */
+  public static void setJoystickPOV(int stick, int pov, DriverStation.POVDirection value) {
+    DriverStationDataJNI.setJoystickPOV(stick, pov, value.value);
+  }
+
+  /**
+   * Sets the maximum index that an axis is available at.
+   *
+   * @param stick The joystick number
+   * @param maximumIndex The maximum index an axis is available at.
+   */
+  public static void setJoystickAxesMaximumIndex(int stick, int maximumIndex) {
+    setJoystickAxesAvailable(stick, (1 << maximumIndex) - 1);
+  }
+
+  /**
+   * Sets the number of axes for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of axes on the indicated joystick
+   */
+  public static void setJoystickAxesAvailable(int stick, int count) {
+    DriverStationDataJNI.setJoystickAxesAvailable(stick, count);
+  }
+
+  /**
+   * Sets the maximum index that a pov is available at.
+   *
+   * @param stick The joystick number
+   * @param maximumIndex The maximum index a pov is available at.
+   */
+  public static void setJoystickPOVsMaximumIndex(int stick, int maximumIndex) {
+    setJoystickPOVsAvailable(stick, (1 << maximumIndex) - 1);
+  }
+
+  /**
+   * Sets the number of POVs for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of POVs on the indicated joystick
+   */
+  public static void setJoystickPOVsAvailable(int stick, int count) {
+    DriverStationDataJNI.setJoystickPOVsAvailable(stick, count);
+  }
+
+  /**
+   * Sets the maximum index that a button is available at.
+   *
+   * @param stick The joystick number
+   * @param maximumIndex The maximum index a button is available at.
+   */
+  public static void setJoystickButtonsMaximumIndex(int stick, int maximumIndex) {
+    if (maximumIndex >= 64) {
+      setJoystickButtonsAvailable(stick, 0xFFFFFFFFFFFFFFFFL);
+    } else {
+      setJoystickButtonsAvailable(stick, (1L << maximumIndex) - 1);
+    }
+  }
+
+  /**
+   * Sets the number of buttons for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of buttons on the indicated joystick
+   */
+  public static void setJoystickButtonsAvailable(int stick, long count) {
+    DriverStationDataJNI.setJoystickButtonsAvailable(stick, count);
+  }
+
+  /**
+   * Sets the value of isGamepad for a joystick.
+   *
+   * @param stick The joystick number
+   * @param isGamepad The value of isGamepad
+   */
+  public static void setJoystickIsGamepad(int stick, boolean isGamepad) {
+    DriverStationDataJNI.setJoystickIsGamepad(stick, isGamepad);
+  }
+
+  /**
+   * Sets the value of type for a joystick.
+   *
+   * @param stick The joystick number
+   * @param type The value of type
+   */
+  public static void setJoystickType(int stick, int type) {
+    DriverStationDataJNI.setJoystickType(stick, type);
+  }
+
+  /**
+   * Sets the name of a joystick.
+   *
+   * @param stick The joystick number
+   * @param name The value of name
+   */
+  public static void setJoystickName(int stick, String name) {
+    DriverStationDataJNI.setJoystickName(stick, name);
+  }
+
+  /**
+   * Sets the game specific message.
+   *
+   * @param message the game specific message
+   */
+  public static void setGameSpecificMessage(String message) {
+    DriverStationDataJNI.setGameSpecificMessage(message);
+  }
+
+  /**
+   * Sets the event name.
+   *
+   * @param name the event name
+   */
+  public static void setEventName(String name) {
+    DriverStationDataJNI.setEventName(name);
+  }
+
+  /**
+   * Sets the match type.
+   *
+   * @param type the match type
+   */
+  public static void setMatchType(DriverStation.MatchType type) {
+    int matchType =
+        switch (type) {
+          case Practice -> 1;
+          case Qualification -> 2;
+          case Elimination -> 3;
+          case None -> 0;
+        };
+    DriverStationDataJNI.setMatchType(matchType);
+  }
+
+  /**
+   * Sets the match number.
+   *
+   * @param matchNumber the match number
+   */
+  public static void setMatchNumber(int matchNumber) {
+    DriverStationDataJNI.setMatchNumber(matchNumber);
+  }
+
+  /**
+   * Sets the replay number.
+   *
+   * @param replayNumber the replay number
+   */
+  public static void setReplayNumber(int replayNumber) {
+    DriverStationDataJNI.setReplayNumber(replayNumber);
+  }
+
+  /** Reset all simulation data for the Driver Station. */
+  public static void resetData() {
+    DriverStationDataJNI.resetData();
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/DutyCycleEncoderSim.java

@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+
+/** Class to control a simulated duty cycle encoder. */
+public class DutyCycleEncoderSim {
+  private final SimDouble m_simPosition;
+  private final SimBoolean m_simIsConnected;
+
+  /**
+   * Constructs from an DutyCycleEncoder object.
+   *
+   * @param encoder DutyCycleEncoder to simulate
+   */
+  public DutyCycleEncoderSim(DutyCycleEncoder encoder) {
+    this(encoder.getSourceChannel());
+  }
+
+  /**
+   * Constructs from a digital input channel.
+   *
+   * @param channel digital input channel.
+   */
+  public DutyCycleEncoderSim(int channel) {
+    SimDeviceSim wrappedSimDevice = new SimDeviceSim("DutyCycle:DutyCycleEncoder", channel);
+    m_simPosition = wrappedSimDevice.getDouble("Position");
+    m_simIsConnected = wrappedSimDevice.getBoolean("Connected");
+  }
+
+  /**
+   * Get the position in turns.
+   *
+   * @return The position.
+   */
+  public double get() {
+    return m_simPosition.get();
+  }
+
+  /**
+   * Set the position.
+   *
+   * @param value The position.
+   */
+  public void set(double value) {
+    m_simPosition.set(value);
+  }
+
+  /**
+   * Get if the encoder is connected.
+   *
+   * @return true if the encoder is connected.
+   */
+  public boolean getConnected() {
+    return m_simIsConnected.get();
+  }
+
+  /**
+   * Set if the encoder is connected.
+   *
+   * @param isConnected Whether or not the sensor is connected.
+   */
+  public void setConnected(boolean isConnected) {
+    m_simIsConnected.set(isConnected);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/DutyCycleSim.java

@@ -0,0 +1,132 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.DutyCycleDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.DutyCycle;
+
+/** Class to control a simulated duty cycle digital input. */
+public class DutyCycleSim {
+  private final int m_index;
+
+  /**
+   * Constructs from a DutyCycle object.
+   *
+   * @param dutyCycle DutyCycle to simulate
+   */
+  public DutyCycleSim(DutyCycle dutyCycle) {
+    m_index = dutyCycle.getSourceChannel();
+  }
+
+  private DutyCycleSim(int index) {
+    m_index = index;
+  }
+
+  /**
+   * Creates a DutyCycleSim for a SmartIO channel.
+   *
+   * @param channel SmartIO channel
+   * @return Simulated object
+   */
+  public static DutyCycleSim createForChannel(int channel) {
+    return new DutyCycleSim(channel);
+  }
+
+  /**
+   * Register a callback to be run when this duty cycle input is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DutyCycleDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelInitializedCallback);
+  }
+
+  /**
+   * Check whether this duty cycle input has been initialized.
+   *
+   * @return true if initialized
+   */
+  public boolean getInitialized() {
+    return DutyCycleDataJNI.getInitialized(m_index);
+  }
+
+  /**
+   * Define whether this duty cycle input has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
+  public void setInitialized(boolean initialized) {
+    DutyCycleDataJNI.setInitialized(m_index, initialized);
+  }
+
+  /**
+   * Register a callback to be run whenever the frequency changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerFrequencyCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DutyCycleDataJNI.registerFrequencyCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelFrequencyCallback);
+  }
+
+  /**
+   * Measure the frequency.
+   *
+   * @return the duty cycle frequency
+   */
+  public double getFrequency() {
+    return DutyCycleDataJNI.getFrequency(m_index);
+  }
+
+  /**
+   * Change the duty cycle frequency.
+   *
+   * @param frequency the new frequency
+   */
+  public void setFrequency(double frequency) {
+    DutyCycleDataJNI.setFrequency(m_index, frequency);
+  }
+
+  /**
+   * Register a callback to be run whenever the output changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerOutputCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DutyCycleDataJNI.registerOutputCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelOutputCallback);
+  }
+
+  /**
+   * Measure the output from this duty cycle port.
+   *
+   * @return the output value
+   */
+  public double getOutput() {
+    return DutyCycleDataJNI.getOutput(m_index);
+  }
+
+  /**
+   * Change the duty cycle output.
+   *
+   * @param output the new output value
+   */
+  public void setOutput(double output) {
+    DutyCycleDataJNI.setOutput(m_index, output);
+  }
+
+  /** Reset all simulation data for the duty cycle output. */
+  public void resetData() {
+    DutyCycleDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/ElevatorSim.java

@@ -0,0 +1,255 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.RobotController;
+
+/** Represents a simulated elevator mechanism. */
+public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
+  // Gearbox for the elevator.
+  private final DCMotor m_gearbox;
+
+  // The min allowable height for the elevator.
+  private final double m_minHeight;
+
+  // The max allowable height for the elevator.
+  private final double m_maxHeight;
+
+  // Whether the simulator should simulate gravity.
+  private final boolean m_simulateGravity;
+
+  /**
+   * Creates a simulated elevator mechanism.
+   *
+   * @param plant The linear system that represents the elevator. This system can be created with
+   *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double,
+   *     double, double)}.
+   * @param gearbox The type of and number of motors in the elevator gearbox.
+   * @param minHeight The min allowable height of the elevator in meters.
+   * @param maxHeight The max allowable height of the elevator in meters.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingHeight The starting height of the elevator in meters.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
+   */
+  @SuppressWarnings("this-escape")
+  public ElevatorSim(
+      LinearSystem<N2, N1, N2> plant,
+      DCMotor gearbox,
+      double minHeight,
+      double maxHeight,
+      boolean simulateGravity,
+      double startingHeight,
+      double... measurementStdDevs) {
+    super(plant, measurementStdDevs);
+    m_gearbox = gearbox;
+    m_minHeight = minHeight;
+    m_maxHeight = maxHeight;
+    m_simulateGravity = simulateGravity;
+
+    setState(startingHeight, 0);
+  }
+
+  /**
+   * Creates a simulated elevator mechanism.
+   *
+   * @param kV The velocity gain.
+   * @param kA The acceleration gain.
+   * @param gearbox The type of and number of motors in the elevator gearbox.
+   * @param minHeight The min allowable height of the elevator in meters.
+   * @param maxHeight The max allowable height of the elevator in meters.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingHeight The starting height of the elevator in meters.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
+   */
+  public ElevatorSim(
+      double kV,
+      double kA,
+      DCMotor gearbox,
+      double minHeight,
+      double maxHeight,
+      boolean simulateGravity,
+      double startingHeight,
+      double... measurementStdDevs) {
+    this(
+        LinearSystemId.identifyPositionSystem(kV, kA),
+        gearbox,
+        minHeight,
+        maxHeight,
+        simulateGravity,
+        startingHeight,
+        measurementStdDevs);
+  }
+
+  /**
+   * Creates a simulated elevator mechanism.
+   *
+   * @param gearbox The type of and number of motors in the elevator gearbox.
+   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+   * @param carriageMass The mass of the elevator carriage in kg.
+   * @param drumRadius The radius of the drum that the elevator spool is wrapped around in meters.
+   * @param minHeight The min allowable height of the elevator in meters.
+   * @param maxHeight The max allowable height of the elevator in meters.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingHeight The starting height of the elevator in meters.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
+   */
+  public ElevatorSim(
+      DCMotor gearbox,
+      double gearing,
+      double carriageMass,
+      double drumRadius,
+      double minHeight,
+      double maxHeight,
+      boolean simulateGravity,
+      double startingHeight,
+      double... measurementStdDevs) {
+    this(
+        LinearSystemId.createElevatorSystem(gearbox, carriageMass, drumRadius, gearing),
+        gearbox,
+        minHeight,
+        maxHeight,
+        simulateGravity,
+        startingHeight,
+        measurementStdDevs);
+  }
+
+  /**
+   * Sets the elevator's state. The new position will be limited between the minimum and maximum
+   * allowed heights.
+   *
+   * @param position The new position in meters.
+   * @param velocity New velocity in meters per second.
+   */
+  public final void setState(double position, double velocity) {
+    setState(VecBuilder.fill(Math.clamp(position, m_minHeight, m_maxHeight), velocity));
+  }
+
+  /**
+   * Returns whether the elevator would hit the lower limit.
+   *
+   * @param elevatorHeight The elevator height in meters.
+   * @return Whether the elevator would hit the lower limit.
+   */
+  public boolean wouldHitLowerLimit(double elevatorHeight) {
+    return elevatorHeight <= this.m_minHeight;
+  }
+
+  /**
+   * Returns whether the elevator would hit the upper limit.
+   *
+   * @param elevatorHeight The elevator height in meters.
+   * @return Whether the elevator would hit the upper limit.
+   */
+  public boolean wouldHitUpperLimit(double elevatorHeight) {
+    return elevatorHeight >= this.m_maxHeight;
+  }
+
+  /**
+   * Returns whether the elevator has hit the lower limit.
+   *
+   * @return Whether the elevator has hit the lower limit.
+   */
+  public boolean hasHitLowerLimit() {
+    return wouldHitLowerLimit(getPosition());
+  }
+
+  /**
+   * Returns whether the elevator has hit the upper limit.
+   *
+   * @return Whether the elevator has hit the upper limit.
+   */
+  public boolean hasHitUpperLimit() {
+    return wouldHitUpperLimit(getPosition());
+  }
+
+  /**
+   * Returns the position of the elevator.
+   *
+   * @return The position of the elevator in meters.
+   */
+  public double getPosition() {
+    return getOutput(0);
+  }
+
+  /**
+   * Returns the velocity of the elevator.
+   *
+   * @return The velocity of the elevator in meters per second.
+   */
+  public double getVelocity() {
+    return getOutput(1);
+  }
+
+  /**
+   * Returns the elevator current draw.
+   *
+   * @return The elevator current draw in amps.
+   */
+  public double getCurrentDraw() {
+    // I = V / R - omega / (Kv * R)
+    // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
+    // spinning 10x faster than the output
+    // v = r w, so w = v/r
+    double kA = 1 / m_plant.getB().get(1, 0);
+    double kV = -m_plant.getA().get(1, 1) * kA;
+    double motorVelocity = m_x.get(1, 0) * kV * m_gearbox.Kv;
+    var appliedVoltage = m_u.get(0, 0);
+    return m_gearbox.getCurrent(motorVelocity, appliedVoltage) * Math.signum(appliedVoltage);
+  }
+
+  /**
+   * Sets the input voltage for the elevator.
+   *
+   * @param volts The input voltage.
+   */
+  public void setInputVoltage(double volts) {
+    setInput(volts);
+    clampInput(RobotController.getBatteryVoltage());
+  }
+
+  /**
+   * Updates the state of the elevator.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u The system inputs (voltage).
+   * @param dt The time difference between controller updates in seconds.
+   */
+  @Override
+  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dt) {
+    // Calculate updated x-hat from Runge-Kutta.
+    var updatedXhat =
+        NumericalIntegration.rkdp(
+            (x, _u) -> {
+              Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
+              if (m_simulateGravity) {
+                xdot = xdot.plus(VecBuilder.fill(0, -9.8));
+              }
+              return xdot;
+            },
+            currentXhat,
+            u,
+            dt);
+
+    // We check for collisions after updating x-hat.
+    if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
+      return VecBuilder.fill(m_minHeight, 0);
+    }
+    if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
+      return VecBuilder.fill(m_maxHeight, 0);
+    }
+    return updatedXhat;
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/EncoderSim.java

@@ -0,0 +1,369 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.EncoderDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.Encoder;
+import java.util.NoSuchElementException;
+
+/** Class to control a simulated encoder. */
+public class EncoderSim {
+  private final int m_index;
+
+  /**
+   * Constructs from an Encoder object.
+   *
+   * @param encoder Encoder to simulate
+   */
+  public EncoderSim(Encoder encoder) {
+    m_index = encoder.getFPGAIndex();
+  }
+
+  private EncoderSim(int index) {
+    m_index = index;
+  }
+
+  /**
+   * Creates an EncoderSim for a digital input channel. Encoders take two channels, so either one
+   * may be specified.
+   *
+   * @param channel digital input channel
+   * @return Simulated object
+   * @throws NoSuchElementException if no Encoder is configured for that channel
+   */
+  public static EncoderSim createForChannel(int channel) {
+    int index = EncoderDataJNI.findForChannel(channel);
+    if (index < 0) {
+      throw new NoSuchElementException("no encoder found for channel " + channel);
+    }
+    return new EncoderSim(index);
+  }
+
+  /**
+   * Creates an EncoderSim for a simulated index. The index is incremented for each simulated
+   * Encoder.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  public static EncoderSim createForIndex(int index) {
+    return new EncoderSim(index);
+  }
+
+  /**
+   * Register a callback on the Initialized property of the encoder.
+   *
+   * @param callback the callback that will be called whenever the Initialized property is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelInitializedCallback);
+  }
+
+  /**
+   * Read the Initialized value of the encoder.
+   *
+   * @return true if initialized
+   */
+  public boolean getInitialized() {
+    return EncoderDataJNI.getInitialized(m_index);
+  }
+
+  /**
+   * Change the Initialized value of the encoder.
+   *
+   * @param initialized the new value
+   */
+  public void setInitialized(boolean initialized) {
+    EncoderDataJNI.setInitialized(m_index, initialized);
+  }
+
+  /**
+   * Register a callback on the count property of the encoder.
+   *
+   * @param callback the callback that will be called whenever the count property is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerCountCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerCountCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelCountCallback);
+  }
+
+  /**
+   * Read the count of the encoder.
+   *
+   * @return the count
+   */
+  public int getCount() {
+    return EncoderDataJNI.getCount(m_index);
+  }
+
+  /**
+   * Change the count of the encoder.
+   *
+   * @param count the new count
+   */
+  public void setCount(int count) {
+    EncoderDataJNI.setCount(m_index, count);
+  }
+
+  /**
+   * Register a callback on the period of the encoder.
+   *
+   * @param callback the callback that will be called whenever the period is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerPeriodCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerPeriodCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelPeriodCallback);
+  }
+
+  /**
+   * Read the period of the encoder.
+   *
+   * @return the encoder period
+   */
+  public double getPeriod() {
+    return EncoderDataJNI.getPeriod(m_index);
+  }
+
+  /**
+   * Change the encoder period.
+   *
+   * @param period the new period
+   */
+  public void setPeriod(double period) {
+    EncoderDataJNI.setPeriod(m_index, period);
+  }
+
+  /**
+   * Register a callback to be called whenever the encoder is reset.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerResetCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerResetCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelResetCallback);
+  }
+
+  /**
+   * Check if the encoder has been reset.
+   *
+   * @return true if reset
+   */
+  public boolean getReset() {
+    return EncoderDataJNI.getReset(m_index);
+  }
+
+  /**
+   * Change the reset property of the encoder.
+   *
+   * @param reset the new value
+   */
+  public void setReset(boolean reset) {
+    EncoderDataJNI.setReset(m_index, reset);
+  }
+
+  /**
+   * Register a callback to be run whenever the max period of the encoder is changed.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerMaxPeriodCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerMaxPeriodCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelMaxPeriodCallback);
+  }
+
+  /**
+   * Get the max period of the encoder.
+   *
+   * @return the max period of the encoder
+   */
+  public double getMaxPeriod() {
+    return EncoderDataJNI.getMaxPeriod(m_index);
+  }
+
+  /**
+   * Change the max period of the encoder.
+   *
+   * @param maxPeriod the new value
+   */
+  public void setMaxPeriod(double maxPeriod) {
+    EncoderDataJNI.setMaxPeriod(m_index, maxPeriod);
+  }
+
+  /**
+   * Register a callback on the direction of the encoder.
+   *
+   * @param callback the callback that will be called whenever the direction is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerDirectionCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerDirectionCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelDirectionCallback);
+  }
+
+  /**
+   * Get the direction of the encoder.
+   *
+   * @return the direction of the encoder
+   */
+  public boolean getDirection() {
+    return EncoderDataJNI.getDirection(m_index);
+  }
+
+  /**
+   * Set the direction of the encoder.
+   *
+   * @param direction the new direction
+   */
+  public void setDirection(boolean direction) {
+    EncoderDataJNI.setDirection(m_index, direction);
+  }
+
+  /**
+   * Register a callback on the reverse direction.
+   *
+   * @param callback the callback that will be called whenever the reverse direction is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerReverseDirectionCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerReverseDirectionCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelReverseDirectionCallback);
+  }
+
+  /**
+   * Get the reverse direction of the encoder.
+   *
+   * @return the reverse direction of the encoder
+   */
+  public boolean getReverseDirection() {
+    return EncoderDataJNI.getReverseDirection(m_index);
+  }
+
+  /**
+   * Set the reverse direction.
+   *
+   * @param reverseDirection the new value
+   */
+  public void setReverseDirection(boolean reverseDirection) {
+    EncoderDataJNI.setReverseDirection(m_index, reverseDirection);
+  }
+
+  /**
+   * Register a callback on the samples-to-average value of this encoder.
+   *
+   * @param callback the callback that will be called whenever the samples-to-average is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerSamplesToAverageCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerSamplesToAverageCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelSamplesToAverageCallback);
+  }
+
+  /**
+   * Get the samples-to-average value.
+   *
+   * @return the samples-to-average value
+   */
+  public int getSamplesToAverage() {
+    return EncoderDataJNI.getSamplesToAverage(m_index);
+  }
+
+  /**
+   * Set the samples-to-average value.
+   *
+   * @param samplesToAverage the new value
+   */
+  public void setSamplesToAverage(int samplesToAverage) {
+    EncoderDataJNI.setSamplesToAverage(m_index, samplesToAverage);
+  }
+
+  /**
+   * Register a callback on the distance per pulse value of this encoder.
+   *
+   * @param callback the callback that will be called whenever the distance per pulse is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerDistancePerPulseCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerDistancePerPulseCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelDistancePerPulseCallback);
+  }
+
+  /**
+   * Get the distance per pulse value.
+   *
+   * @return the distance per pulse value
+   */
+  public double getDistancePerPulse() {
+    return EncoderDataJNI.getDistancePerPulse(m_index);
+  }
+
+  /**
+   * Set the distance per pulse value.
+   *
+   * @param distancePerPulse the new distancePerPulse
+   */
+  public void setDistancePerPulse(double distancePerPulse) {
+    EncoderDataJNI.setDistancePerPulse(m_index, distancePerPulse);
+  }
+
+  /**
+   * Change the encoder distance.
+   *
+   * @param distance the new distance
+   */
+  public void setDistance(double distance) {
+    EncoderDataJNI.setDistance(m_index, distance);
+  }
+
+  /**
+   * Read the distance of the encoder.
+   *
+   * @return the encoder distance
+   */
+  public double getDistance() {
+    return EncoderDataJNI.getDistance(m_index);
+  }
+
+  /**
+   * Change the rate of the encoder.
+   *
+   * @param rate the new rate
+   */
+  public void setRate(double rate) {
+    EncoderDataJNI.setRate(m_index, rate);
+  }
+
+  /**
+   * Get the rate of the encoder.
+   *
+   * @return the rate of change
+   */
+  public double getRate() {
+    return EncoderDataJNI.getRate(m_index);
+  }
+
+  /** Resets all simulation data for this encoder. */
+  public void resetData() {
+    EncoderDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/FlywheelSim.java

@@ -0,0 +1,156 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.RobotController;
+
+/** Represents a simulated flywheel mechanism. */
+public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
+  // Gearbox for the flywheel.
+  private final DCMotor m_gearbox;
+
+  // The gearing from the motors to the output.
+  private final double m_gearing;
+
+  // The moment of inertia for the flywheel mechanism.
+  private final double m_j;
+
+  /**
+   * Creates a simulated flywheel mechanism.
+   *
+   * @param plant The linear system that represents the flywheel. Use either {@link
+   *     LinearSystemId#createFlywheelSystem(DCMotor, double, double)} if using physical constants
+   *     or {@link LinearSystemId#identifyVelocitySystem(double, double)} if using system
+   *     characterization.
+   * @param gearbox The type of and number of motors in the flywheel gearbox.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for velocity.
+   */
+  public FlywheelSim(
+      LinearSystem<N1, N1, N1> plant, DCMotor gearbox, double... measurementStdDevs) {
+    super(plant, measurementStdDevs);
+    m_gearbox = gearbox;
+
+    // By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf,
+    // the flywheel state-space model is:
+    //
+    //   dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u
+    //   A = -G²Kₜ/(KᵥRJ)
+    //   B = GKₜ/(RJ)
+    //
+    // Solve for G.
+    //
+    //   A/B = -G/Kᵥ
+    //   G = -KᵥA/B
+    //
+    // Solve for J.
+    //
+    //   B = GKₜ/(RJ)
+    //   J = GKₜ/(RB)
+    m_gearing = -gearbox.Kv * plant.getA(0, 0) / plant.getB(0, 0);
+    m_j = m_gearing * gearbox.Kt / (gearbox.R * plant.getB(0, 0));
+  }
+
+  /**
+   * Sets the flywheel's angular velocity.
+   *
+   * @param velocity The new velocity in radians per second.
+   */
+  public void setAngularVelocity(double velocity) {
+    setState(VecBuilder.fill(velocity));
+  }
+
+  /**
+   * Returns the gear ratio of the flywheel.
+   *
+   * @return the flywheel's gear ratio.
+   */
+  public double getGearing() {
+    return m_gearing;
+  }
+
+  /**
+   * Returns the moment of inertia.
+   *
+   * @return The flywheel's moment of inertia in kg-m².
+   */
+  public double getJ() {
+    return m_j;
+  }
+
+  /**
+   * Returns the gearbox for the flywheel.
+   *
+   * @return The flywheel's gearbox.
+   */
+  public DCMotor getGearbox() {
+    return m_gearbox;
+  }
+
+  /**
+   * Returns the flywheel's velocity.
+   *
+   * @return The flywheel's velocity in rad/s.
+   */
+  public double getAngularVelocity() {
+    return getOutput(0);
+  }
+
+  /**
+   * Returns the flywheel's acceleration.
+   *
+   * @return The flywheel's acceleration in rad/s².
+   */
+  public double getAngularAcceleration() {
+    var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
+    return acceleration.get(0, 0);
+  }
+
+  /**
+   * Returns the flywheel's torque.
+   *
+   * @return The flywheel's torque in Newton-meters.
+   */
+  public double getTorque() {
+    return getAngularAcceleration() * m_j;
+  }
+
+  /**
+   * Returns the flywheel's current draw.
+   *
+   * @return The flywheel's current draw in amps.
+   */
+  public double getCurrentDraw() {
+    // I = V / R - omega / (Kv * R)
+    // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
+    // 2x faster than the flywheel
+    return m_gearbox.getCurrent(m_x.get(0, 0) * m_gearing, m_u.get(0, 0))
+        * Math.signum(m_u.get(0, 0));
+  }
+
+  /**
+   * Gets the input voltage for the flywheel.
+   *
+   * @return The flywheel's input voltage.
+   */
+  public double getInputVoltage() {
+    return getInput(0);
+  }
+
+  /**
+   * Sets the input voltage for the flywheel.
+   *
+   * @param volts The input voltage.
+   */
+  public void setInputVoltage(double volts) {
+    setInput(volts);
+    clampInput(RobotController.getBatteryVoltage());
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/GamepadSim.java

@@ -0,0 +1,324 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.Gamepad;
+
+/** Class to control a simulated Gamepad controller. */
+public class GamepadSim extends GenericHIDSim {
+  /**
+   * Constructs from a Gamepad object.
+   *
+   * @param joystick controller to simulate
+   */
+  @SuppressWarnings("this-escape")
+  public GamepadSim(Gamepad joystick) {
+    super(joystick);
+    setAxesMaximumIndex(6);
+    setButtonsMaximumIndex(26);
+    setPOVsMaximumIndex(1);
+  }
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  @SuppressWarnings("this-escape")
+  public GamepadSim(int port) {
+    super(port);
+    setAxesMaximumIndex(6);
+    setButtonsMaximumIndex(26);
+    setPOVsMaximumIndex(1);
+  }
+
+  /**
+   * Change the left X value of the controller's joystick.
+   *
+   * @param value the new value
+   */
+  public void setLeftX(double value) {
+    setRawAxis(Gamepad.Axis.kLeftX.value, value);
+  }
+
+  /**
+   * Change the left Y value of the controller's joystick.
+   *
+   * @param value the new value
+   */
+  public void setLeftY(double value) {
+    setRawAxis(Gamepad.Axis.kLeftY.value, value);
+  }
+
+  /**
+   * Change the right X value of the controller's joystick.
+   *
+   * @param value the new value
+   */
+  public void setRightX(double value) {
+    setRawAxis(Gamepad.Axis.kRightX.value, value);
+  }
+
+  /**
+   * Change the right Y value of the controller's joystick.
+   *
+   * @param value the new value
+   */
+  public void setRightY(double value) {
+    setRawAxis(Gamepad.Axis.kRightY.value, value);
+  }
+
+  /**
+   * Change the value of the left trigger axis on the controller.
+   *
+   * @param value the new value
+   */
+  public void setLeftTriggerAxis(double value) {
+    setRawAxis(Gamepad.Axis.kLeftTrigger.value, value);
+  }
+
+  /**
+   * Change the value of the right trigger axis on the controller.
+   *
+   * @param value the new value
+   */
+  public void setRightTriggerAxis(double value) {
+    setRawAxis(Gamepad.Axis.kRightTrigger.value, value);
+  }
+
+  /**
+   * Change the value of the South Face button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setSouthFaceButton(boolean value) {
+    setRawButton(Gamepad.Button.kSouthFace.value, value);
+  }
+
+  /**
+   * Change the value of the East Face button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setEastFaceButton(boolean value) {
+    setRawButton(Gamepad.Button.kEastFace.value, value);
+  }
+
+  /**
+   * Change the value of the West Face button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setWestFaceButton(boolean value) {
+    setRawButton(Gamepad.Button.kWestFace.value, value);
+  }
+
+  /**
+   * Change the value of the North Face button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setNorthFaceButton(boolean value) {
+    setRawButton(Gamepad.Button.kNorthFace.value, value);
+  }
+
+  /**
+   * Change the value of the Back button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setBackButton(boolean value) {
+    setRawButton(Gamepad.Button.kBack.value, value);
+  }
+
+  /**
+   * Change the value of the Guide button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setGuideButton(boolean value) {
+    setRawButton(Gamepad.Button.kGuide.value, value);
+  }
+
+  /**
+   * Change the value of the Start button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setStartButton(boolean value) {
+    setRawButton(Gamepad.Button.kStart.value, value);
+  }
+
+  /**
+   * Change the value of the left stick button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setLeftStickButton(boolean value) {
+    setRawButton(Gamepad.Button.kLeftStick.value, value);
+  }
+
+  /**
+   * Change the value of the right stick button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setRightStickButton(boolean value) {
+    setRawButton(Gamepad.Button.kRightStick.value, value);
+  }
+
+  /**
+   * Change the value of the right shoulder button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setLeftShoulderButton(boolean value) {
+    setRawButton(Gamepad.Button.kLeftShoulder.value, value);
+  }
+
+  /**
+   * Change the value of the right shoulder button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setRightShoulderButton(boolean value) {
+    setRawButton(Gamepad.Button.kRightShoulder.value, value);
+  }
+
+  /**
+   * Change the value of the D-pad up button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setDpadUpButton(boolean value) {
+    setRawButton(Gamepad.Button.kDpadUp.value, value);
+  }
+
+  /**
+   * Change the value of the D-pad down button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setDpadDownButton(boolean value) {
+    setRawButton(Gamepad.Button.kDpadDown.value, value);
+  }
+
+  /**
+   * Change the value of the D-pad left button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setDpadLeftButton(boolean value) {
+    setRawButton(Gamepad.Button.kDpadLeft.value, value);
+  }
+
+  /**
+   * Change the value of the D-pad right button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setDpadRightButton(boolean value) {
+    setRawButton(Gamepad.Button.kDpadRight.value, value);
+  }
+
+  /**
+   * Change the value of the Miscellaneous 1 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setMisc1Button(boolean value) {
+    setRawButton(Gamepad.Button.kMisc1.value, value);
+  }
+
+  /**
+   * Change the value of the Right Paddle 1 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setRightPaddle1Button(boolean value) {
+    setRawButton(Gamepad.Button.kRightPaddle1.value, value);
+  }
+
+  /**
+   * Change the value of the Left Paddle 1 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setLeftPaddle1Button(boolean value) {
+    setRawButton(Gamepad.Button.kLeftPaddle1.value, value);
+  }
+
+  /**
+   * Change the value of the Right Paddle 2 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setRightPaddle2Button(boolean value) {
+    setRawButton(Gamepad.Button.kRightPaddle2.value, value);
+  }
+
+  /**
+   * Change the value of the Left Paddle 2 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setLeftPaddle2Button(boolean value) {
+    setRawButton(Gamepad.Button.kLeftPaddle2.value, value);
+  }
+
+  /**
+   * Change the value of the Touchpad button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setTouchpadButton(boolean value) {
+    setRawButton(Gamepad.Button.kTouchpad.value, value);
+  }
+
+  /**
+   * Change the value of the Miscellaneous 2 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setMisc2Button(boolean value) {
+    setRawButton(Gamepad.Button.kMisc2.value, value);
+  }
+
+  /**
+   * Change the value of the Miscellaneous 3 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setMisc3Button(boolean value) {
+    setRawButton(Gamepad.Button.kMisc3.value, value);
+  }
+
+  /**
+   * Change the value of the Miscellaneous 4 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setMisc4Button(boolean value) {
+    setRawButton(Gamepad.Button.kMisc4.value, value);
+  }
+
+  /**
+   * Change the value of the Miscellaneous 5 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setMisc5Button(boolean value) {
+    setRawButton(Gamepad.Button.kMisc5.value, value);
+  }
+
+  /**
+   * Change the value of the Miscellaneous 6 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setMisc6Button(boolean value) {
+    setRawButton(Gamepad.Button.kMisc6.value, value);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/GenericHIDSim.java

@@ -0,0 +1,181 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.GenericHID;
+
+/** Class to control a simulated generic joystick. */
+public class GenericHIDSim {
+  /** GenericHID port. */
+  protected final int m_port;
+
+  /**
+   * Constructs from a GenericHID object.
+   *
+   * @param joystick joystick to simulate
+   */
+  public GenericHIDSim(GenericHID joystick) {
+    m_port = joystick.getPort();
+  }
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  public GenericHIDSim(int port) {
+    m_port = port;
+  }
+
+  /** Updates joystick data so that new values are visible to the user program. */
+  public void notifyNewData() {
+    DriverStationSim.notifyNewData();
+  }
+
+  /**
+   * Set the value of a given button.
+   *
+   * @param button the button to set
+   * @param value the new value
+   */
+  public void setRawButton(int button, boolean value) {
+    DriverStationSim.setJoystickButton(m_port, button, value);
+  }
+
+  /**
+   * Set the value of a given axis.
+   *
+   * @param axis the axis to set
+   * @param value the new value
+   */
+  public void setRawAxis(int axis, double value) {
+    DriverStationSim.setJoystickAxis(m_port, axis, value);
+  }
+
+  /**
+   * Set the value of a given POV.
+   *
+   * @param pov the POV to set
+   * @param value the new value
+   */
+  public void setPOV(int pov, DriverStation.POVDirection value) {
+    DriverStationSim.setJoystickPOV(m_port, pov, value);
+  }
+
+  /**
+   * Set the value of the default POV (port 0).
+   *
+   * @param value the new value
+   */
+  public void setPOV(DriverStation.POVDirection value) {
+    setPOV(0, value);
+  }
+
+  /**
+   * Set the maximum axis index for this device.
+   *
+   * @param maximumIndex the new maximum axis index
+   */
+  public void setAxesMaximumIndex(int maximumIndex) {
+    DriverStationSim.setJoystickAxesMaximumIndex(m_port, maximumIndex);
+  }
+
+  /**
+   * Set the axis count of this device.
+   *
+   * @param count the new axis count
+   */
+  public void setAxesAvailable(int count) {
+    DriverStationSim.setJoystickAxesAvailable(m_port, count);
+  }
+
+  /**
+   * Set the maximum POV index for this device.
+   *
+   * @param maximumIndex the new maximum POV index
+   */
+  public void setPOVsMaximumIndex(int maximumIndex) {
+    DriverStationSim.setJoystickPOVsMaximumIndex(m_port, maximumIndex);
+  }
+
+  /**
+   * Set the POV count of this device.
+   *
+   * @param count the new POV count
+   */
+  public void setPOVsAvailable(int count) {
+    DriverStationSim.setJoystickPOVsAvailable(m_port, count);
+  }
+
+  /**
+   * Set the maximum button index for this device.
+   *
+   * @param maximumIndex the new maximum button index
+   */
+  public void setButtonsMaximumIndex(int maximumIndex) {
+    DriverStationSim.setJoystickButtonsMaximumIndex(m_port, maximumIndex);
+  }
+
+  /**
+   * Set the button count of this device.
+   *
+   * @param count the new button count
+   */
+  public void setButtonsAvailable(long count) {
+    DriverStationSim.setJoystickButtonsAvailable(m_port, count);
+  }
+
+  /**
+   * Set the type of this device.
+   *
+   * @param type the new device type
+   */
+  public void setType(GenericHID.HIDType type) {
+    DriverStationSim.setJoystickType(m_port, type.value);
+  }
+
+  /**
+   * Set the name of this device.
+   *
+   * @param name the new device name
+   */
+  public void setName(String name) {
+    DriverStationSim.setJoystickName(m_port, name);
+  }
+
+  /**
+   * Read the output of a button.
+   *
+   * @param outputNumber the button number
+   * @return the value of the button (true = pressed)
+   */
+  public boolean getOutput(int outputNumber) {
+    long outputs = getOutputs();
+    return (outputs & (1L << (outputNumber - 1))) != 0;
+  }
+
+  /**
+   * Get the encoded 16-bit integer that passes button values.
+   *
+   * @return the button values
+   */
+  public long getOutputs() {
+    return DriverStationSim.getJoystickOutputs(m_port);
+  }
+
+  /**
+   * Get the joystick rumble.
+   *
+   * @param type the rumble to read
+   * @return the rumble value
+   */
+  public double getRumble(GenericHID.RumbleType type) {
+    int value =
+        DriverStationSim.getJoystickRumble(
+            m_port, type == GenericHID.RumbleType.kLeftRumble ? 0 : 1);
+    return value / 65535.0;
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/I2CSim.java

@@ -0,0 +1,81 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.BufferCallback;
+import edu.wpi.first.hal.simulation.ConstBufferCallback;
+import edu.wpi.first.hal.simulation.I2CDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+
+/** A class to control a simulated I2C device. */
+public class I2CSim {
+  private final int m_index;
+
+  /**
+   * Construct a new I2C simulation object.
+   *
+   * @param index the HAL index of the I2C object
+   */
+  public I2CSim(int index) {
+    m_index = index;
+  }
+
+  /**
+   * Register a callback to be run when this I2C device is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = I2CDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, I2CDataJNI::cancelInitializedCallback);
+  }
+
+  /**
+   * Check whether this I2C device has been initialized.
+   *
+   * @return true if initialized
+   */
+  public boolean getInitialized() {
+    return I2CDataJNI.getInitialized(m_index);
+  }
+
+  /**
+   * Define whether this I2C device has been initialized.
+   *
+   * @param initialized whether this device is initialized
+   */
+  public void setInitialized(boolean initialized) {
+    I2CDataJNI.setInitialized(m_index, initialized);
+  }
+
+  /**
+   * Register a callback to be run whenever a `read` operation is done.
+   *
+   * @param callback the callback that is run on `read` operations
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerReadCallback(BufferCallback callback) {
+    int uid = I2CDataJNI.registerReadCallback(m_index, callback);
+    return new CallbackStore(m_index, uid, I2CDataJNI::cancelReadCallback);
+  }
+
+  /**
+   * Register a callback to be run whenever a `write` operation is done.
+   *
+   * @param callback the callback that is run on `write` operations
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerWriteCallback(ConstBufferCallback callback) {
+    int uid = I2CDataJNI.registerWriteCallback(m_index, callback);
+    return new CallbackStore(m_index, uid, I2CDataJNI::cancelWriteCallback);
+  }
+
+  /** Reset all I2C simulation data. */
+  public void resetData() {
+    I2CDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/JoystickSim.java

@@ -0,0 +1,107 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.Joystick;
+
+/** Class to control a simulated joystick. */
+public class JoystickSim extends GenericHIDSim {
+  private Joystick m_joystick;
+
+  /**
+   * Constructs from a Joystick object.
+   *
+   * @param joystick joystick to simulate
+   */
+  @SuppressWarnings("this-escape")
+  public JoystickSim(Joystick joystick) {
+    super(joystick);
+    m_joystick = joystick;
+    // default to a reasonable joystick configuration
+    setAxesMaximumIndex(5);
+    setButtonsMaximumIndex(12);
+    setPOVsMaximumIndex(1);
+  }
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  @SuppressWarnings("this-escape")
+  public JoystickSim(int port) {
+    super(port);
+    // default to a reasonable joystick configuration
+    setAxesMaximumIndex(5);
+    setButtonsMaximumIndex(12);
+    setPOVsMaximumIndex(1);
+  }
+
+  /**
+   * Set the X value of the joystick.
+   *
+   * @param value the new X value
+   */
+  public void setX(double value) {
+    setRawAxis(m_joystick != null ? m_joystick.getXChannel() : Joystick.kDefaultXChannel, value);
+  }
+
+  /**
+   * Set the Y value of the joystick.
+   *
+   * @param value the new Y value
+   */
+  public void setY(double value) {
+    setRawAxis(m_joystick != null ? m_joystick.getYChannel() : Joystick.kDefaultYChannel, value);
+  }
+
+  /**
+   * Set the Z value of the joystick.
+   *
+   * @param value the new Z value
+   */
+  public void setZ(double value) {
+    setRawAxis(m_joystick != null ? m_joystick.getZChannel() : Joystick.kDefaultZChannel, value);
+  }
+
+  /**
+   * Set the twist value of the joystick.
+   *
+   * @param value the new twist value
+   */
+  public void setTwist(double value) {
+    setRawAxis(
+        m_joystick != null ? m_joystick.getTwistChannel() : Joystick.kDefaultTwistChannel, value);
+  }
+
+  /**
+   * Set the throttle value of the joystick.
+   *
+   * @param value the new throttle value
+   */
+  public void setThrottle(double value) {
+    setRawAxis(
+        m_joystick != null ? m_joystick.getThrottleChannel() : Joystick.kDefaultThrottleChannel,
+        value);
+  }
+
+  /**
+   * Set the trigger value of the joystick.
+   *
+   * @param state the new value
+   */
+  public void setTrigger(boolean state) {
+    setRawButton(1, state);
+  }
+
+  /**
+   * Set the top state of the joystick.
+   *
+   * @param state the new state
+   */
+  public void setTop(boolean state) {
+    setRawButton(2, state);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/LinearSystemSim.java

@@ -0,0 +1,196 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import org.ejml.MatrixDimensionException;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * This class helps simulate linear systems. To use this class, do the following in the {@link
+ * edu.wpi.first.wpilibj.IterativeRobotBase#simulationPeriodic} method.
+ *
+ * <p>Call {@link #setInput(double...)} with the inputs to the system (usually voltage).
+ *
+ * <p>Call {@link #update} to update the simulation.
+ *
+ * <p>Set simulated sensor readings with the simulated positions in {@link #getOutput()}
+ *
+ * @param <States> Number of states of the system.
+ * @param <Inputs> Number of inputs to the system.
+ * @param <Outputs> Number of outputs of the system.
+ */
+public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs extends Num> {
+  /** The plant that represents the linear system. */
+  protected final LinearSystem<States, Inputs, Outputs> m_plant;
+
+  /** State vector. */
+  protected Matrix<States, N1> m_x;
+
+  /** Input vector. */
+  protected Matrix<Inputs, N1> m_u;
+
+  /** Output vector. */
+  protected Matrix<Outputs, N1> m_y;
+
+  /** The standard deviations of measurements, used for adding noise to the measurements. */
+  protected final Matrix<Outputs, N1> m_measurementStdDevs;
+
+  /**
+   * Creates a simulated generic linear system with measurement noise.
+   *
+   * @param system The system being controlled.
+   * @param measurementStdDevs Standard deviations of measurements. Can be empty if no noise is
+   *     desired. If present must have same number of items as Outputs
+   */
+  public LinearSystemSim(
+      LinearSystem<States, Inputs, Outputs> system, double... measurementStdDevs) {
+    if (measurementStdDevs.length != 0 && measurementStdDevs.length != system.getC().getNumRows()) {
+      throw new MatrixDimensionException(
+          "Malformed measurementStdDevs! Got "
+              + measurementStdDevs.length
+              + " elements instead of "
+              + system.getC().getNumRows());
+    }
+    this.m_plant = system;
+    if (measurementStdDevs.length == 0) {
+      m_measurementStdDevs = new Matrix<>(new SimpleMatrix(system.getC().getNumRows(), 1));
+    } else {
+      m_measurementStdDevs = new Matrix<>(new SimpleMatrix(measurementStdDevs));
+    }
+    m_x = new Matrix<>(new SimpleMatrix(system.getA().getNumRows(), 1));
+    m_u = new Matrix<>(new SimpleMatrix(system.getB().getNumCols(), 1));
+    m_y = new Matrix<>(new SimpleMatrix(system.getC().getNumRows(), 1));
+  }
+
+  /**
+   * Updates the simulation.
+   *
+   * @param dt The time between updates in seconds.
+   */
+  public void update(double dt) {
+    // Update x. By default, this is the linear system dynamics xₖ₊₁ = Axₖ + Buₖ.
+    m_x = updateX(m_x, m_u, dt);
+
+    // yₖ = Cxₖ + Duₖ
+    m_y = m_plant.calculateY(m_x, m_u);
+
+    // Add measurement noise.
+    if (m_measurementStdDevs != null) {
+      m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
+    }
+  }
+
+  /**
+   * Returns the current output of the plant.
+   *
+   * @return The current output of the plant.
+   */
+  public Matrix<Outputs, N1> getOutput() {
+    return m_y;
+  }
+
+  /**
+   * Returns an element of the current output of the plant.
+   *
+   * @param row The row to return.
+   * @return An element of the current output of the plant.
+   */
+  public double getOutput(int row) {
+    return m_y.get(row, 0);
+  }
+
+  /**
+   * Sets the system inputs (usually voltages).
+   *
+   * @param u The system inputs.
+   */
+  public void setInput(Matrix<Inputs, N1> u) {
+    this.m_u = u;
+  }
+
+  /**
+   * Sets the system inputs.
+   *
+   * @param row The row in the input matrix to set.
+   * @param value The value to set the row to.
+   */
+  public void setInput(int row, double value) {
+    m_u.set(row, 0, value);
+  }
+
+  /**
+   * Sets the system inputs.
+   *
+   * @param u An array of doubles that represent the inputs of the system.
+   */
+  public void setInput(double... u) {
+    if (u.length != m_u.getNumRows()) {
+      throw new MatrixDimensionException(
+          "Malformed input! Got " + u.length + " elements instead of " + m_u.getNumRows());
+    }
+    m_u = new Matrix<>(new SimpleMatrix(m_u.getNumRows(), 1, true, u));
+  }
+
+  /**
+   * Returns the current input of the plant.
+   *
+   * @return The current input of the plant.
+   */
+  public Matrix<Inputs, N1> getInput() {
+    return m_u;
+  }
+
+  /**
+   * Returns an element of the current input of the plant.
+   *
+   * @param row The row to return.
+   * @return An element of the current input of the plant.
+   */
+  public double getInput(int row) {
+    return m_u.get(row, 0);
+  }
+
+  /**
+   * Sets the system state.
+   *
+   * @param state The new state.
+   */
+  public void setState(Matrix<States, N1> state) {
+    m_x = state;
+
+    // Update the output to reflect the new state.
+    //
+    //   yₖ = Cxₖ + Duₖ
+    m_y = m_plant.calculateY(m_x, m_u);
+  }
+
+  /**
+   * Updates the state estimate of the system.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u The system inputs (usually voltage).
+   * @param dt The time difference between controller updates in seconds.
+   * @return The new state.
+   */
+  protected Matrix<States, N1> updateX(
+      Matrix<States, N1> currentXhat, Matrix<Inputs, N1> u, double dt) {
+    return m_plant.calculateX(currentXhat, u, dt);
+  }
+
+  /**
+   * Clamp the input vector such that no element exceeds the maximum allowed value. If any does, the
+   * relative magnitudes of the input will be maintained.
+   *
+   * @param maxInput The maximum magnitude of the input vector after clamping.
+   */
+  protected void clampInput(double maxInput) {
+    m_u = StateSpaceUtil.desaturateInputVector(m_u, maxInput);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/NotifierSim.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifierDataJNI;
+
+/** Class to control simulated notifiers. */
+public final class NotifierSim {
+  private NotifierSim() {}
+
+  /**
+   * Gets the timeout of the next notifier.
+   *
+   * @return Timestamp
+   */
+  public static long getNextTimeout() {
+    return NotifierDataJNI.getNextTimeout();
+  }
+
+  /**
+   * Gets the total number of notifiers.
+   *
+   * @return Count
+   */
+  public static int getNumNotifiers() {
+    return NotifierDataJNI.getNumNotifiers();
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/PDPSim.java

@@ -0,0 +1,170 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.PowerDistributionDataJNI;
+import edu.wpi.first.wpilibj.PowerDistribution;
+
+/** Class to control a simulated Power Distribution Panel (PDP). */
+public class PDPSim {
+  private final int m_index;
+
+  /** Constructs for the default PDP. */
+  public PDPSim() {
+    m_index = 0;
+  }
+
+  /**
+   * Constructs from a PDP module number (CAN ID).
+   *
+   * @param module module number
+   */
+  public PDPSim(int module) {
+    m_index = module;
+  }
+
+  /**
+   * Constructs from a PowerDistribution object.
+   *
+   * @param pdp PowerDistribution to simulate
+   */
+  public PDPSim(PowerDistribution pdp) {
+    m_index = pdp.getModule();
+  }
+
+  /**
+   * Register a callback to be run when the PDP is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid =
+        PowerDistributionDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PowerDistributionDataJNI::cancelInitializedCallback);
+  }
+
+  /**
+   * Check whether the PDP has been initialized.
+   *
+   * @return true if initialized
+   */
+  public boolean getInitialized() {
+    return PowerDistributionDataJNI.getInitialized(m_index);
+  }
+
+  /**
+   * Define whether the PDP has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
+  public void setInitialized(boolean initialized) {
+    PowerDistributionDataJNI.setInitialized(m_index, initialized);
+  }
+
+  /**
+   * Register a callback to be run whenever the PDP temperature changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerTemperatureCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid =
+        PowerDistributionDataJNI.registerTemperatureCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PowerDistributionDataJNI::cancelTemperatureCallback);
+  }
+
+  /**
+   * Check the temperature of the PDP.
+   *
+   * @return the PDP temperature
+   */
+  public double getTemperature() {
+    return PowerDistributionDataJNI.getTemperature(m_index);
+  }
+
+  /**
+   * Define the PDP temperature.
+   *
+   * @param temperature the new PDP temperature
+   */
+  public void setTemperature(double temperature) {
+    PowerDistributionDataJNI.setTemperature(m_index, temperature);
+  }
+
+  /**
+   * Register a callback to be run whenever the PDP voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerVoltageCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PowerDistributionDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PowerDistributionDataJNI::cancelVoltageCallback);
+  }
+
+  /**
+   * Check the PDP voltage.
+   *
+   * @return the PDP voltage.
+   */
+  public double getVoltage() {
+    return PowerDistributionDataJNI.getVoltage(m_index);
+  }
+
+  /**
+   * Set the PDP voltage.
+   *
+   * @param voltage the new PDP voltage
+   */
+  public void setVoltage(double voltage) {
+    PowerDistributionDataJNI.setVoltage(m_index, voltage);
+  }
+
+  /**
+   * Register a callback to be run whenever the current of a specific channel changes.
+   *
+   * @param channel the channel
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerCurrentCallback(
+      int channel, NotifyCallback callback, boolean initialNotify) {
+    int uid =
+        PowerDistributionDataJNI.registerCurrentCallback(m_index, channel, callback, initialNotify);
+    return new CallbackStore(
+        m_index, channel, uid, PowerDistributionDataJNI::cancelCurrentCallback);
+  }
+
+  /**
+   * Read the current in one of the PDP channels.
+   *
+   * @param channel the channel to check
+   * @return the current in the given channel
+   */
+  public double getCurrent(int channel) {
+    return PowerDistributionDataJNI.getCurrent(m_index, channel);
+  }
+
+  /**
+   * Change the current in the given channel.
+   *
+   * @param channel the channel to edit
+   * @param current the new current for the channel
+   */
+  public void setCurrent(int channel, double current) {
+    PowerDistributionDataJNI.setCurrent(m_index, channel, current);
+  }
+
+  /** Reset all PDP simulation data. */
+  public void resetData() {
+    PowerDistributionDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/PWMMotorControllerSim.java

@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController;
+
+/** Class to control a simulated PWM motor controller. */
+public class PWMMotorControllerSim {
+  private final SimDouble m_simSpeed;
+
+  /**
+   * Constructor.
+   *
+   * @param motorctrl The device to simulate
+   */
+  public PWMMotorControllerSim(PWMMotorController motorctrl) {
+    this(motorctrl.getChannel());
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param channel The channel the motor controller is attached to.
+   */
+  public PWMMotorControllerSim(int channel) {
+    SimDeviceSim simDevice = new SimDeviceSim("PWMMotorController", channel);
+    m_simSpeed = simDevice.getDouble("Speed");
+  }
+
+  /**
+   * Gets the motor speed set.
+   *
+   * @return Speed
+   */
+  public double getSpeed() {
+    return m_simSpeed.get();
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/PWMSim.java

@@ -0,0 +1,129 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.PWMDataJNI;
+import edu.wpi.first.wpilibj.PWM;
+
+/** Class to control a simulated PWM output. */
+public class PWMSim {
+  private final int m_index;
+
+  /**
+   * Constructs from a PWM object.
+   *
+   * @param pwm PWM to simulate
+   */
+  public PWMSim(PWM pwm) {
+    m_index = pwm.getChannel();
+  }
+
+  /**
+   * Constructs from a PWM channel number.
+   *
+   * @param channel Channel number
+   */
+  public PWMSim(int channel) {
+    m_index = channel;
+  }
+
+  /**
+   * Register a callback to be run when the PWM is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PWMDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PWMDataJNI::cancelInitializedCallback);
+  }
+
+  /**
+   * Check whether the PWM has been initialized.
+   *
+   * @return true if initialized
+   */
+  public boolean getInitialized() {
+    return PWMDataJNI.getInitialized(m_index);
+  }
+
+  /**
+   * Define whether the PWM has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
+  public void setInitialized(boolean initialized) {
+    PWMDataJNI.setInitialized(m_index, initialized);
+  }
+
+  /**
+   * Register a callback to be run when the PWM raw value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerPulseMicrosecondCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = PWMDataJNI.registerPulseMicrosecondCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PWMDataJNI::cancelPulseMicrosecondCallback);
+  }
+
+  /**
+   * Get the PWM pulse microsecond value.
+   *
+   * @return the PWM pulse microsecond value
+   */
+  public int getPulseMicrosecond() {
+    return PWMDataJNI.getPulseMicrosecond(m_index);
+  }
+
+  /**
+   * Set the PWM pulse microsecond value.
+   *
+   * @param microsecondPulseTime the PWM pulse microsecond value
+   */
+  public void setPulseMicrosecond(int microsecondPulseTime) {
+    PWMDataJNI.setPulseMicrosecond(m_index, microsecondPulseTime);
+  }
+
+  /**
+   * Register a callback to be run when the PWM period scale changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerOutputPeriodCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = PWMDataJNI.registerOutputPeriodCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PWMDataJNI::cancelOutputPeriodCallback);
+  }
+
+  /**
+   * Get the PWM period scale.
+   *
+   * @return the PWM period scale
+   */
+  public int getOutputPeriod() {
+    return PWMDataJNI.getOutputPeriod(m_index);
+  }
+
+  /**
+   * Set the PWM period scale.
+   *
+   * @param period the PWM period scale
+   */
+  public void setOutputPeriod(int period) {
+    PWMDataJNI.setOutputPeriod(m_index, period);
+  }
+
+  /** Reset all simulation data. */
+  public void resetData() {
+    PWMDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/PneumaticsBaseSim.java

@@ -0,0 +1,173 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.PneumaticsBase;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+
+/** Common base class for pneumatics module simulation classes. */
+public abstract class PneumaticsBaseSim {
+  /** PneumaticsBase index. */
+  protected final int m_index;
+
+  /**
+   * Get a module sim for a specific type.
+   *
+   * @param module the module number / CAN ID.
+   * @param type the module type.
+   * @return the module object.
+   */
+  public static PneumaticsBaseSim getForType(int module, PneumaticsModuleType type) {
+    return switch (type) {
+      case CTREPCM -> new CTREPCMSim(module);
+      case REVPH -> new REVPHSim(module);
+    };
+  }
+
+  /**
+   * Constructs a PneumaticsBaseSim with the given index.
+   *
+   * @param index The index.
+   */
+  protected PneumaticsBaseSim(int index) {
+    m_index = index;
+  }
+
+  /**
+   * Constructs a PneumaticsBaseSim for the given module.
+   *
+   * @param module The module.
+   */
+  protected PneumaticsBaseSim(PneumaticsBase module) {
+    this(module.getModuleNumber());
+  }
+
+  /**
+   * Check whether the PCM/PH has been initialized.
+   *
+   * @return true if initialized
+   */
+  public abstract boolean getInitialized();
+
+  /**
+   * Define whether the PCM/PH has been initialized.
+   *
+   * @param initialized true for initialized
+   */
+  public abstract void setInitialized(boolean initialized);
+
+  /**
+   * Register a callback to be run when the PCM/PH is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public abstract CallbackStore registerInitializedCallback(
+      NotifyCallback callback, boolean initialNotify);
+
+  /**
+   * Check if the compressor is on.
+   *
+   * @return true if the compressor is active
+   */
+  public abstract boolean getCompressorOn();
+
+  /**
+   * Set whether the compressor is active.
+   *
+   * @param compressorOn the new value
+   */
+  public abstract void setCompressorOn(boolean compressorOn);
+
+  /**
+   * Register a callback to be run when the compressor activates.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public abstract CallbackStore registerCompressorOnCallback(
+      NotifyCallback callback, boolean initialNotify);
+
+  /**
+   * Check the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @return the solenoid output
+   */
+  public abstract boolean getSolenoidOutput(int channel);
+
+  /**
+   * Change the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @param solenoidOutput the new solenoid output
+   */
+  public abstract void setSolenoidOutput(int channel, boolean solenoidOutput);
+
+  /**
+   * Register a callback to be run when the solenoid output on a channel changes.
+   *
+   * @param channel the channel to monitor
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public abstract CallbackStore registerSolenoidOutputCallback(
+      int channel, NotifyCallback callback, boolean initialNotify);
+
+  /**
+   * Check the value of the pressure switch.
+   *
+   * @return the pressure switch value
+   */
+  public abstract boolean getPressureSwitch();
+
+  /**
+   * Set the value of the pressure switch.
+   *
+   * @param pressureSwitch the new value
+   */
+  public abstract void setPressureSwitch(boolean pressureSwitch);
+
+  /**
+   * Register a callback to be run whenever the pressure switch value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public abstract CallbackStore registerPressureSwitchCallback(
+      NotifyCallback callback, boolean initialNotify);
+
+  /**
+   * Read the compressor current.
+   *
+   * @return the current of the compressor connected to this module
+   */
+  public abstract double getCompressorCurrent();
+
+  /**
+   * Set the compressor current.
+   *
+   * @param compressorCurrent the new compressor current
+   */
+  public abstract void setCompressorCurrent(double compressorCurrent);
+
+  /**
+   * Register a callback to be run whenever the compressor current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public abstract CallbackStore registerCompressorCurrentCallback(
+      NotifyCallback callback, boolean initialNotify);
+
+  /** Reset all simulation data for this object. */
+  public abstract void resetData();
+}

wpilibj/src/main/java/org/wpilib/simulation/REVPHSim.java

@@ -0,0 +1,157 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.REVPHDataJNI;
+import edu.wpi.first.wpilibj.PneumaticHub;
+import edu.wpi.first.wpilibj.SensorUtil;
+
+/** Class to control a simulated PneumaticHub (PH). */
+public class REVPHSim extends PneumaticsBaseSim {
+  /** Constructs for the default PH. */
+  public REVPHSim() {
+    super(SensorUtil.getDefaultREVPHModule());
+  }
+
+  /**
+   * Constructs from a PH module number (CAN ID).
+   *
+   * @param module module number
+   */
+  public REVPHSim(int module) {
+    super(module);
+  }
+
+  /**
+   * Constructs from a PneumaticHum object.
+   *
+   * @param module PCM module to simulate
+   */
+  public REVPHSim(PneumaticHub module) {
+    super(module);
+  }
+
+  /**
+   * Check whether the closed loop compressor control is active.
+   *
+   * @return config type
+   */
+  public int getCompressorConfigType() {
+    return REVPHDataJNI.getCompressorConfigType(m_index);
+  }
+
+  /**
+   * Turn on/off the closed loop control of the compressor.
+   *
+   * @param compressorConfigType compressor config type
+   */
+  public void setCompressorConfigType(int compressorConfigType) {
+    REVPHDataJNI.setCompressorConfigType(m_index, compressorConfigType);
+  }
+
+  /**
+   * Register a callback to be run whenever the closed loop state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerCompressorConfigTypeCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = REVPHDataJNI.registerCompressorConfigTypeCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, REVPHDataJNI::cancelCompressorConfigTypeCallback);
+  }
+
+  @Override
+  public boolean getInitialized() {
+    return REVPHDataJNI.getInitialized(m_index);
+  }
+
+  @Override
+  public void setInitialized(boolean initialized) {
+    REVPHDataJNI.setInitialized(m_index, initialized);
+  }
+
+  @Override
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = REVPHDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, REVPHDataJNI::cancelInitializedCallback);
+  }
+
+  @Override
+  public boolean getCompressorOn() {
+    return REVPHDataJNI.getCompressorOn(m_index);
+  }
+
+  @Override
+  public void setCompressorOn(boolean compressorOn) {
+    REVPHDataJNI.setCompressorOn(m_index, compressorOn);
+  }
+
+  @Override
+  public CallbackStore registerCompressorOnCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = REVPHDataJNI.registerCompressorOnCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, REVPHDataJNI::cancelCompressorOnCallback);
+  }
+
+  @Override
+  public boolean getSolenoidOutput(int channel) {
+    return REVPHDataJNI.getSolenoidOutput(m_index, channel);
+  }
+
+  @Override
+  public void setSolenoidOutput(int channel, boolean solenoidOutput) {
+    REVPHDataJNI.setSolenoidOutput(m_index, channel, solenoidOutput);
+  }
+
+  @Override
+  public CallbackStore registerSolenoidOutputCallback(
+      int channel, NotifyCallback callback, boolean initialNotify) {
+    int uid =
+        REVPHDataJNI.registerSolenoidOutputCallback(m_index, channel, callback, initialNotify);
+    return new CallbackStore(m_index, channel, uid, REVPHDataJNI::cancelSolenoidOutputCallback);
+  }
+
+  @Override
+  public boolean getPressureSwitch() {
+    return REVPHDataJNI.getPressureSwitch(m_index);
+  }
+
+  @Override
+  public void setPressureSwitch(boolean pressureSwitch) {
+    REVPHDataJNI.setPressureSwitch(m_index, pressureSwitch);
+  }
+
+  @Override
+  public CallbackStore registerPressureSwitchCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = REVPHDataJNI.registerPressureSwitchCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, REVPHDataJNI::cancelPressureSwitchCallback);
+  }
+
+  @Override
+  public double getCompressorCurrent() {
+    return REVPHDataJNI.getCompressorCurrent(m_index);
+  }
+
+  @Override
+  public void setCompressorCurrent(double compressorCurrent) {
+    REVPHDataJNI.setCompressorCurrent(m_index, compressorCurrent);
+  }
+
+  @Override
+  public CallbackStore registerCompressorCurrentCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = REVPHDataJNI.registerCompressorCurrentCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, REVPHDataJNI::cancelCompressorCurrentCallback);
+  }
+
+  @Override
+  public void resetData() {
+    REVPHDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/RoboRioSim.java

@@ -0,0 +1,304 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.RoboRioDataJNI;
+
+/** A utility class to control a simulated RoboRIO. */
+public final class RoboRioSim {
+  private RoboRioSim() {
+    // Utility class
+  }
+
+  /**
+   * Register a callback to be run whenever the Vin voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerVInVoltageCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerVInVoltageCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelVInVoltageCallback);
+  }
+
+  /**
+   * Measure the Vin voltage.
+   *
+   * @return the Vin voltage
+   */
+  public static double getVInVoltage() {
+    return RoboRioDataJNI.getVInVoltage();
+  }
+
+  /**
+   * Define the Vin voltage.
+   *
+   * @param vInVoltage the new voltage
+   */
+  public static void setVInVoltage(double vInVoltage) {
+    RoboRioDataJNI.setVInVoltage(vInVoltage);
+  }
+
+  /**
+   * Register a callback to be run whenever the 3.3V rail voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerUserVoltage3V3Callback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserVoltage3V3Callback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserVoltage3V3Callback);
+  }
+
+  /**
+   * Measure the 3.3V rail voltage.
+   *
+   * @return the 3.3V rail voltage
+   */
+  public static double getUserVoltage3V3() {
+    return RoboRioDataJNI.getUserVoltage3V3();
+  }
+
+  /**
+   * Define the 3.3V rail voltage.
+   *
+   * @param userVoltage3V3 the new voltage
+   */
+  public static void setUserVoltage3V3(double userVoltage3V3) {
+    RoboRioDataJNI.setUserVoltage3V3(userVoltage3V3);
+  }
+
+  /**
+   * Register a callback to be run whenever the 3.3V rail current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerUserCurrent3V3Callback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserCurrent3V3Callback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserCurrent3V3Callback);
+  }
+
+  /**
+   * Measure the 3.3V rail current.
+   *
+   * @return the 3.3V rail current
+   */
+  public static double getUserCurrent3V3() {
+    return RoboRioDataJNI.getUserCurrent3V3();
+  }
+
+  /**
+   * Define the 3.3V rail current.
+   *
+   * @param userCurrent3V3 the new current
+   */
+  public static void setUserCurrent3V3(double userCurrent3V3) {
+    RoboRioDataJNI.setUserCurrent3V3(userCurrent3V3);
+  }
+
+  /**
+   * Register a callback to be run whenever the 3.3V rail active state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerUserActive3V3Callback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserActive3V3Callback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserActive3V3Callback);
+  }
+
+  /**
+   * Get the 3.3V rail active state.
+   *
+   * @return true if the 3.3V rail is active
+   */
+  public static boolean getUserActive3V3() {
+    return RoboRioDataJNI.getUserActive3V3();
+  }
+
+  /**
+   * Set the 3.3V rail active state.
+   *
+   * @param userActive3V3 true to make rail active
+   */
+  public static void setUserActive3V3(boolean userActive3V3) {
+    RoboRioDataJNI.setUserActive3V3(userActive3V3);
+  }
+
+  /**
+   * Register a callback to be run whenever the 3.3V rail number of faults changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerUserFaults3V3Callback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserFaults3V3Callback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserFaults3V3Callback);
+  }
+
+  /**
+   * Get the 3.3V rail number of faults.
+   *
+   * @return number of faults
+   */
+  public static int getUserFaults3V3() {
+    return RoboRioDataJNI.getUserFaults3V3();
+  }
+
+  /**
+   * Set the 3.3V rail number of faults.
+   *
+   * @param userFaults3V3 number of faults
+   */
+  public static void setUserFaults3V3(int userFaults3V3) {
+    RoboRioDataJNI.setUserFaults3V3(userFaults3V3);
+  }
+
+  /**
+   * Register a callback to be run whenever the Brownout voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerBrownoutVoltageCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerBrownoutVoltageCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelBrownoutVoltageCallback);
+  }
+
+  /**
+   * Measure the Brownout voltage.
+   *
+   * @return the Brownout voltage
+   */
+  public static double getBrownoutVoltage() {
+    return RoboRioDataJNI.getBrownoutVoltage();
+  }
+
+  /**
+   * Define the Brownout voltage.
+   *
+   * @param vInVoltage the new voltage
+   */
+  public static void setBrownoutVoltage(double vInVoltage) {
+    RoboRioDataJNI.setBrownoutVoltage(vInVoltage);
+  }
+
+  /**
+   * Register a callback to be run whenever the cpu temp changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerCPUTempCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerCPUTempCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelCPUTempCallback);
+  }
+
+  /**
+   * Get the cpu temp.
+   *
+   * @return the cpu temp.
+   */
+  public static double getCPUTemp() {
+    return RoboRioDataJNI.getCPUTemp();
+  }
+
+  /**
+   * Set the cpu temp.
+   *
+   * @param cpuTemp the new cpu temp.
+   */
+  public static void setCPUTemp(double cpuTemp) {
+    RoboRioDataJNI.setCPUTemp(cpuTemp);
+  }
+
+  /**
+   * Register a callback to be run whenever the team number changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerTeamNumberCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerTeamNumberCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelTeamNumberCallback);
+  }
+
+  /**
+   * Get the team number.
+   *
+   * @return the team number.
+   */
+  public static int getTeamNumber() {
+    return RoboRioDataJNI.getTeamNumber();
+  }
+
+  /**
+   * Set the team number.
+   *
+   * @param teamNumber the new team number.
+   */
+  public static void setTeamNumber(int teamNumber) {
+    RoboRioDataJNI.setTeamNumber(teamNumber);
+  }
+
+  /**
+   * Get the serial number.
+   *
+   * @return The serial number.
+   */
+  public static String getSerialNumber() {
+    return RoboRioDataJNI.getSerialNumber();
+  }
+
+  /**
+   * Set the serial number.
+   *
+   * @param serialNumber The serial number.
+   */
+  public static void setSerialNumber(String serialNumber) {
+    RoboRioDataJNI.setSerialNumber(serialNumber);
+  }
+
+  /**
+   * Get the comments string.
+   *
+   * @return The comments string.
+   */
+  public static String getComments() {
+    return RoboRioDataJNI.getComments();
+  }
+
+  /**
+   * Set the comments string.
+   *
+   * @param comments The comments string.
+   */
+  public static void setComments(String comments) {
+    RoboRioDataJNI.setComments(comments);
+  }
+
+  /** Reset all simulation data. */
+  public static void resetData() {
+    RoboRioDataJNI.resetData();
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/SendableChooserSim.java

@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.wpilibj.RobotBase;
+
+/** Class that facilitates control of a SendableChooser's selected option in simulation. */
+public class SendableChooserSim implements AutoCloseable {
+  private StringPublisher m_publisher;
+
+  /**
+   * Constructs a SendableChooserSim.
+   *
+   * @param path The path where the SendableChooser is published.
+   */
+  public SendableChooserSim(String path) {
+    this(NetworkTableInstance.getDefault(), path);
+  }
+
+  /**
+   * Constructs a SendableChooserSim.
+   *
+   * @param inst The NetworkTables instance.
+   * @param path The path where the SendableChooser is published.
+   */
+  public SendableChooserSim(NetworkTableInstance inst, String path) {
+    if (RobotBase.isSimulation()) {
+      m_publisher = inst.getStringTopic(path + "selected").publish();
+    }
+  }
+
+  @Override
+  public void close() {
+    if (RobotBase.isSimulation()) {
+      m_publisher.close();
+    }
+  }
+
+  /**
+   * Set the selected option.
+   *
+   * @param option The option.
+   */
+  public void setSelected(String option) {
+    if (RobotBase.isSimulation()) {
+      m_publisher.set(option);
+    }
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/SharpIRSim.java

@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.SharpIR;
+
+/** Simulation class for Sharp IR sensors. */
+public class SharpIRSim {
+  private final SimDouble m_simRange;
+
+  /**
+   * Constructor.
+   *
+   * @param sharpIR The real sensor to simulate
+   */
+  public SharpIRSim(SharpIR sharpIR) {
+    this(sharpIR.getChannel());
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param channel Analog channel for this sensor
+   */
+  public SharpIRSim(int channel) {
+    SimDeviceSim simDevice = new SimDeviceSim("SharpIR", channel);
+    m_simRange = simDevice.getDouble("Range (m)");
+  }
+
+  /**
+   * Set the range in meters returned by the distance sensor.
+   *
+   * @param range range in meters of the target returned by the sensor
+   */
+  public void setRange(double range) {
+    m_simRange.set(range);
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/SimDeviceSim.java

@@ -0,0 +1,270 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.hal.SimInt;
+import edu.wpi.first.hal.SimLong;
+import edu.wpi.first.hal.SimValue;
+import edu.wpi.first.hal.simulation.SimDeviceCallback;
+import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
+import edu.wpi.first.hal.simulation.SimValueCallback;
+
+/** Class to control the simulation side of a SimDevice. */
+public class SimDeviceSim {
+  private final int m_handle;
+
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   */
+  public SimDeviceSim(String name) {
+    this(SimDeviceDataJNI.getSimDeviceHandle(name));
+  }
+
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   * @param index device index number to append to name
+   */
+  public SimDeviceSim(String name, int index) {
+    this(name + "[" + index + "]");
+  }
+
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   * @param index device index number to append to name
+   * @param channel device channel number to append to name
+   */
+  public SimDeviceSim(String name, int index, int channel) {
+    this(name + "[" + index + "," + channel + "]");
+  }
+
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param handle the low level handle for the corresponding SimDevice
+   */
+  public SimDeviceSim(int handle) {
+    m_handle = handle;
+  }
+
+  /**
+   * Get the name of this object.
+   *
+   * @return name
+   */
+  public String getName() {
+    return SimDeviceDataJNI.getSimDeviceName(m_handle);
+  }
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  public SimValue getValue(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimValue(handle);
+  }
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  public SimInt getInt(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimInt(handle);
+  }
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  public SimLong getLong(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimLong(handle);
+  }
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  public SimDouble getDouble(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimDouble(handle);
+  }
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  public SimEnum getEnum(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimEnum(handle);
+  }
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  public SimBoolean getBoolean(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimBoolean(handle);
+  }
+
+  /**
+   * Get all options for the given enum.
+   *
+   * @param val the enum
+   * @return names of the different values for that enum
+   */
+  public static String[] getEnumOptions(SimEnum val) {
+    return SimDeviceDataJNI.getSimValueEnumOptions(val.getNativeHandle());
+  }
+
+  /**
+   * Get all data of this object.
+   *
+   * @return all data and fields of this object
+   */
+  public SimDeviceDataJNI.SimValueInfo[] enumerateValues() {
+    return SimDeviceDataJNI.enumerateSimValues(m_handle);
+  }
+
+  /**
+   * Get the native handle of this object.
+   *
+   * @return the handle used to refer to this object through JNI
+   */
+  public int getNativeHandle() {
+    return m_handle;
+  }
+
+  /**
+   * Register a callback to be run every time a new value is added to this device.
+   *
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerValueCreatedCallback(
+      SimValueCallback callback, boolean initialNotify) {
+    int uid = SimDeviceDataJNI.registerSimValueCreatedCallback(m_handle, callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueCreatedCallback);
+  }
+
+  /**
+   * Register a callback to be run every time a value is changed on this device.
+   *
+   * @param value simulated value
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerValueChangedCallback(
+      SimValue value, SimValueCallback callback, boolean initialNotify) {
+    int uid =
+        SimDeviceDataJNI.registerSimValueChangedCallback(
+            value.getNativeHandle(), callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueChangedCallback);
+  }
+
+  /**
+   * Register a callback for SimDouble.reset() and similar functions. The callback is called with
+   * the old value.
+   *
+   * @param value simulated value
+   * @param callback callback
+   * @param initialNotify ignored (present for consistency)
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerValueResetCallback(
+      SimValue value, SimValueCallback callback, boolean initialNotify) {
+    int uid =
+        SimDeviceDataJNI.registerSimValueResetCallback(
+            value.getNativeHandle(), callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueResetCallback);
+  }
+
+  /**
+   * Get all sim devices with the given prefix.
+   *
+   * @param prefix the prefix to filter sim devices
+   * @return all sim devices
+   */
+  public static SimDeviceDataJNI.SimDeviceInfo[] enumerateDevices(String prefix) {
+    return SimDeviceDataJNI.enumerateSimDevices(prefix);
+  }
+
+  /**
+   * Register a callback to be run every time a new {@link edu.wpi.first.hal.SimDevice} is created.
+   *
+   * @param prefix the prefix to filter sim devices
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerDeviceCreatedCallback(
+      String prefix, SimDeviceCallback callback, boolean initialNotify) {
+    int uid = SimDeviceDataJNI.registerSimDeviceCreatedCallback(prefix, callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceCreatedCallback);
+  }
+
+  /**
+   * Register a callback to be run every time a {@link edu.wpi.first.hal.SimDevice} is
+   * freed/destroyed.
+   *
+   * @param prefix the prefix to filter sim devices
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public static CallbackStore registerDeviceFreedCallback(
+      String prefix, SimDeviceCallback callback, boolean initialNotify) {
+    int uid = SimDeviceDataJNI.registerSimDeviceFreedCallback(prefix, callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceFreedCallback);
+  }
+
+  /** Reset all SimDevice data. */
+  public static void resetData() {
+    SimDeviceDataJNI.resetSimDeviceData();
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java

@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.SimulatorJNI;
+
+/** Simulation hooks. */
+public final class SimHooks {
+  private SimHooks() {}
+
+  /**
+   * Override the HAL runtime type (simulated/real).
+   *
+   * @param type runtime type
+   */
+  public static void setHALRuntimeType(int type) {
+    SimulatorJNI.setRuntimeType(type);
+  }
+
+  /** Waits until the user program has started. */
+  public static void waitForProgramStart() {
+    SimulatorJNI.waitForProgramStart();
+  }
+
+  /** Sets that the user program has started. */
+  public static void setProgramStarted() {
+    SimulatorJNI.setProgramStarted();
+  }
+
+  /**
+   * Returns true if the user program has started.
+   *
+   * @return True if the user program has started.
+   */
+  public static boolean getProgramStarted() {
+    return SimulatorJNI.getProgramStarted();
+  }
+
+  /** Restart the simulator time. */
+  public static void restartTiming() {
+    SimulatorJNI.restartTiming();
+  }
+
+  /** Pause the simulator time. */
+  public static void pauseTiming() {
+    SimulatorJNI.pauseTiming();
+  }
+
+  /** Resume the simulator time. */
+  public static void resumeTiming() {
+    SimulatorJNI.resumeTiming();
+  }
+
+  /**
+   * Check if the simulator time is paused.
+   *
+   * @return true if paused
+   */
+  public static boolean isTimingPaused() {
+    return SimulatorJNI.isTimingPaused();
+  }
+
+  /**
+   * Advance the simulator time and wait for all notifiers to run.
+   *
+   * @param delta the amount to advance in seconds
+   */
+  public static void stepTiming(double delta) {
+    SimulatorJNI.stepTiming((long) (delta * 1e6));
+  }
+
+  /**
+   * Advance the simulator time and return immediately.
+   *
+   * @param delta the amount to advance in seconds
+   */
+  public static void stepTimingAsync(double delta) {
+    SimulatorJNI.stepTimingAsync((long) (delta * 1e6));
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/SingleJointedArmSim.java

@@ -0,0 +1,267 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.RobotController;
+
+/** Represents a simulated single jointed arm mechanism. */
+public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
+  // The gearbox for the arm.
+  private final DCMotor m_gearbox;
+
+  // The gearing between the motors and the output.
+  private final double m_gearing;
+
+  // The length of the arm.
+  private final double m_armLength;
+
+  // The minimum angle that the arm is capable of.
+  private final double m_minAngle;
+
+  // The maximum angle that the arm is capable of.
+  private final double m_maxAngle;
+
+  // Whether the simulator should simulate gravity.
+  private final boolean m_simulateGravity;
+
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param plant The linear system that represents the arm. This system can be created with {@link
+   *     edu.wpi.first.math.system.plant.LinearSystemId#createSingleJointedArmSystem(DCMotor,
+   *     double, double)}.
+   * @param gearbox The type of and number of motors in the arm gearbox.
+   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param armLength The length of the arm in meters.
+   * @param minAngleRads The minimum angle that the arm is capable of.
+   * @param maxAngleRads The maximum angle that the arm is capable of.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingAngleRads The initial position of the Arm simulation in radians.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
+   */
+  @SuppressWarnings("this-escape")
+  public SingleJointedArmSim(
+      LinearSystem<N2, N1, N2> plant,
+      DCMotor gearbox,
+      double gearing,
+      double armLength,
+      double minAngleRads,
+      double maxAngleRads,
+      boolean simulateGravity,
+      double startingAngleRads,
+      double... measurementStdDevs) {
+    super(plant, measurementStdDevs);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+    m_armLength = armLength;
+    m_minAngle = minAngleRads;
+    m_maxAngle = maxAngleRads;
+    m_simulateGravity = simulateGravity;
+
+    setState(startingAngleRads, 0.0);
+  }
+
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param gearbox The type of and number of motors in the arm gearbox.
+   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param j The moment of inertia of the arm in kg-m²; can be calculated from CAD software.
+   * @param armLength The length of the arm in meters.
+   * @param minAngleRads The minimum angle that the arm is capable of.
+   * @param maxAngleRads The maximum angle that the arm is capable of.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingAngleRads The initial position of the Arm simulation in radians.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
+   */
+  public SingleJointedArmSim(
+      DCMotor gearbox,
+      double gearing,
+      double j,
+      double armLength,
+      double minAngleRads,
+      double maxAngleRads,
+      boolean simulateGravity,
+      double startingAngleRads,
+      double... measurementStdDevs) {
+    this(
+        LinearSystemId.createSingleJointedArmSystem(gearbox, j, gearing),
+        gearbox,
+        gearing,
+        armLength,
+        minAngleRads,
+        maxAngleRads,
+        simulateGravity,
+        startingAngleRads,
+        measurementStdDevs);
+  }
+
+  /**
+   * Sets the arm's state. The new angle will be limited between the minimum and maximum allowed
+   * limits.
+   *
+   * @param angleRadians The new angle in radians.
+   * @param velocityRadPerSec The new angular velocity in radians per second.
+   */
+  public final void setState(double angleRadians, double velocityRadPerSec) {
+    setState(VecBuilder.fill(Math.clamp(angleRadians, m_minAngle, m_maxAngle), velocityRadPerSec));
+  }
+
+  /**
+   * Returns whether the arm would hit the lower limit.
+   *
+   * @param currentAngleRads The current arm height.
+   * @return Whether the arm would hit the lower limit.
+   */
+  public boolean wouldHitLowerLimit(double currentAngleRads) {
+    return currentAngleRads <= this.m_minAngle;
+  }
+
+  /**
+   * Returns whether the arm would hit the upper limit.
+   *
+   * @param currentAngleRads The current arm height.
+   * @return Whether the arm would hit the upper limit.
+   */
+  public boolean wouldHitUpperLimit(double currentAngleRads) {
+    return currentAngleRads >= this.m_maxAngle;
+  }
+
+  /**
+   * Returns whether the arm has hit the lower limit.
+   *
+   * @return Whether the arm has hit the lower limit.
+   */
+  public boolean hasHitLowerLimit() {
+    return wouldHitLowerLimit(getAngle());
+  }
+
+  /**
+   * Returns whether the arm has hit the upper limit.
+   *
+   * @return Whether the arm has hit the upper limit.
+   */
+  public boolean hasHitUpperLimit() {
+    return wouldHitUpperLimit(getAngle());
+  }
+
+  /**
+   * Returns the current arm angle.
+   *
+   * @return The current arm angle in radians.
+   */
+  public double getAngle() {
+    return getOutput(0);
+  }
+
+  /**
+   * Returns the current arm velocity.
+   *
+   * @return The current arm velocity in radians per second.
+   */
+  public double getVelocity() {
+    return getOutput(1);
+  }
+
+  /**
+   * Returns the arm current draw.
+   *
+   * @return The arm current draw in amps.
+   */
+  public double getCurrentDraw() {
+    // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
+    // spinning 10x faster than the output
+    var motorVelocity = m_x.get(1, 0) * m_gearing;
+    return m_gearbox.getCurrent(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0));
+  }
+
+  /**
+   * Sets the input voltage for the arm.
+   *
+   * @param volts The input voltage.
+   */
+  public void setInputVoltage(double volts) {
+    setInput(volts);
+    clampInput(RobotController.getBatteryVoltage());
+  }
+
+  /**
+   * Calculates a rough estimate of the moment of inertia of an arm given its length and mass.
+   *
+   * @param length The length of the arm in m.
+   * @param mass The mass of the arm in kg.
+   * @return The calculated moment of inertia in kg-m².
+   */
+  public static double estimateMOI(double length, double mass) {
+    return 1.0 / 3.0 * mass * length * length;
+  }
+
+  /**
+   * Updates the state of the arm.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u The system inputs (voltage).
+   * @param dt The time difference between controller updates in seconds.
+   */
+  @Override
+  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dt) {
+    // The torque on the arm is given by τ = F⋅r, where F is the force applied by
+    // gravity and r the distance from pivot to center of mass. Recall from
+    // dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is
+    // torque on the arm, J is the mass-moment of inertia about the pivot axis,
+    // and α is the angular acceleration in rad/s². Rearranging yields: α = F⋅r/J
+    //
+    // We substitute in F = m⋅g⋅cos(θ), where θ is the angle from horizontal:
+    //
+    //   α = (m⋅g⋅cos(θ))⋅r/J
+    //
+    // Multiply RHS by cos(θ) to account for the arm angle. Further, we know the
+    // arm mass-moment of inertia J of our arm is given by J=1/3 mL², modeled as a
+    // rod rotating about it's end, where L is the overall rod length. The mass
+    // distribution is assumed to be uniform. Substitute r=L/2 to find:
+    //
+    //   α = (m⋅g⋅cos(θ))⋅r/(1/3 mL²)
+    //   α = (m⋅g⋅cos(θ))⋅(L/2)/(1/3 mL²)
+    //   α = 3/2⋅g⋅cos(θ)/L
+    //
+    // This acceleration is next added to the linear system dynamics ẋ=Ax+Bu
+    //
+    //   f(x, u) = Ax + Bu + [0  α]ᵀ
+    //   f(x, u) = Ax + Bu + [0  3/2⋅g⋅cos(θ)/L]ᵀ
+
+    Matrix<N2, N1> updatedXhat =
+        NumericalIntegration.rkdp(
+            (Matrix<N2, N1> x, Matrix<N1, N1> _u) -> {
+              Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
+              if (m_simulateGravity) {
+                double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLength;
+                xdot = xdot.plus(VecBuilder.fill(0, alphaGrav));
+              }
+              return xdot;
+            },
+            currentXhat,
+            u,
+            dt);
+
+    // We check for collision after updating xhat
+    if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
+      return VecBuilder.fill(m_minAngle, 0);
+    }
+    if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
+      return VecBuilder.fill(m_maxAngle, 0);
+    }
+    return updatedXhat;
+  }
+}

wpilibj/src/main/java/org/wpilib/simulation/SolenoidSim.java

@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.PneumaticsBase;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+
+/** Class to control a simulated {@link edu.wpi.first.wpilibj.Solenoid}. */
+public class SolenoidSim {
+  private final PneumaticsBaseSim m_module;
+  private final int m_channel;
+
+  /**
+   * Constructs for a solenoid on the given pneumatics module.
+   *
+   * @param moduleSim the PCM the solenoid is connected to.
+   * @param channel the solenoid channel.
+   */
+  public SolenoidSim(PneumaticsBaseSim moduleSim, int channel) {
+    m_module = moduleSim;
+    m_channel = channel;
+  }
+
+  /**
+   * Constructs for a solenoid on a pneumatics module of the given type and ID.
+   *
+   * @param module the CAN ID of the pneumatics module the solenoid is connected to.
+   * @param moduleType the module type (PH or PCM)
+   * @param channel the solenoid channel.
+   */
+  public SolenoidSim(int module, PneumaticsModuleType moduleType, int channel) {
+    this(PneumaticsBaseSim.getForType(module, moduleType), channel);
+  }
+
+  /**
+   * Constructs for a solenoid on a pneumatics module of the given type and default ID.
+   *
+   * @param moduleType the module type (PH or PCM)
+   * @param channel the solenoid channel.
+   */
+  public SolenoidSim(PneumaticsModuleType moduleType, int channel) {
+    this(PneumaticsBase.getDefaultForType(moduleType), moduleType, channel);
+  }
+
+  /**
+   * Check the solenoid output.
+   *
+   * @return the solenoid output
+   */
+  public boolean getOutput() {
+    return m_module.getSolenoidOutput(m_channel);
+  }
+
+  /**
+   * Change the solenoid output.
+   *
+   * @param output the new solenoid output
+   */
+  public void setOutput(boolean output) {
+    m_module.setSolenoidOutput(m_channel, output);
+  }
+
+  /**
+   * Register a callback to be run when the output of this solenoid has changed.
+   *
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  public CallbackStore registerOutputCallback(NotifyCallback callback, boolean initialNotify) {
+    return m_module.registerSolenoidOutputCallback(m_channel, callback, initialNotify);
+  }
+
+  /**
+   * Get the wrapped {@link PneumaticsBaseSim} object.
+   *
+   * @return the wrapped {@link PneumaticsBaseSim} object.
+   */
+  public PneumaticsBaseSim getPCMSim() {
+    return m_module;
+  }
+}

wpilibj/src/main/java/org/wpilib/smartdashboard/Field2d.java

@@ -0,0 +1,140 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * 2D representation of game field for dashboards.
+ *
+ * <p>An object's pose is the location shown on the dashboard view. Note that for the robot, this
+ * may or may not match the internal odometry. For example, the robot is shown at a particular
+ * starting location, the pose in this class would represent the actual location on the field, but
+ * the robot's internal state might have a 0,0,0 pose (unless it's initialized to something
+ * different).
+ *
+ * <p>As the user is able to edit the pose, code performing updates should get the robot pose,
+ * transform it as appropriate (e.g. based on wheel odometry), and set the new pose.
+ *
+ * <p>This class provides methods to set the robot pose, but other objects can also be shown by
+ * using the getObject() function. Other objects can also have multiple poses (which will show the
+ * object at multiple locations).
+ */
+public class Field2d implements NTSendable, AutoCloseable {
+  /** Constructor. */
+  @SuppressWarnings("this-escape")
+  public Field2d() {
+    FieldObject2d obj = new FieldObject2d("Robot");
+    obj.setPose(Pose2d.kZero);
+    m_objects.add(obj);
+    SendableRegistry.add(this, "Field");
+  }
+
+  @Override
+  public void close() {
+    for (FieldObject2d obj : m_objects) {
+      obj.close();
+    }
+  }
+
+  /**
+   * Set the robot pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  public synchronized void setRobotPose(Pose2d pose) {
+    m_objects.get(0).setPose(pose);
+  }
+
+  /**
+   * Set the robot pose from x, y, and rotation.
+   *
+   * @param x X location, in meters
+   * @param y Y location, in meters
+   * @param rotation rotation
+   */
+  public synchronized void setRobotPose(double x, double y, Rotation2d rotation) {
+    m_objects.get(0).setPose(x, y, rotation);
+  }
+
+  /**
+   * Set the robot pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  public synchronized void setRobotPose(Distance x, Distance y, Rotation2d rotation) {
+    m_objects.get(0).setPose(x.in(Meters), y.in(Meters), rotation);
+  }
+
+  /**
+   * Get the robot pose.
+   *
+   * @return 2D pose
+   */
+  public synchronized Pose2d getRobotPose() {
+    return m_objects.get(0).getPose();
+  }
+
+  /**
+   * Get or create a field object.
+   *
+   * @param name The field object's name.
+   * @return Field object
+   */
+  public synchronized FieldObject2d getObject(String name) {
+    for (FieldObject2d obj : m_objects) {
+      if (obj.m_name.equals(name)) {
+        return obj;
+      }
+    }
+    FieldObject2d obj = new FieldObject2d(name);
+    m_objects.add(obj);
+    if (m_table != null) {
+      synchronized (obj) {
+        obj.m_entry = m_table.getDoubleArrayTopic(name).getEntry(new double[] {});
+      }
+    }
+    return obj;
+  }
+
+  /**
+   * Get the robot object.
+   *
+   * @return Field object for robot
+   */
+  public synchronized FieldObject2d getRobotObject() {
+    return m_objects.get(0);
+  }
+
+  @Override
+  public void initSendable(NTSendableBuilder builder) {
+    builder.setSmartDashboardType("Field2d");
+
+    synchronized (this) {
+      m_table = builder.getTable();
+      for (FieldObject2d obj : m_objects) {
+        synchronized (obj) {
+          obj.m_entry = m_table.getDoubleArrayTopic(obj.m_name).getEntry(new double[] {});
+          obj.updateEntry(true);
+        }
+      }
+    }
+  }
+
+  private NetworkTable m_table;
+  private final List<FieldObject2d> m_objects = new ArrayList<>();
+}

wpilibj/src/main/java/org/wpilib/smartdashboard/FieldObject2d.java

@@ -0,0 +1,173 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.networktables.DoubleArrayEntry;
+import edu.wpi.first.units.measure.Distance;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+/** Game field object on a Field2d. */
+public class FieldObject2d implements AutoCloseable {
+  /**
+   * Package-local constructor.
+   *
+   * @param name name
+   */
+  FieldObject2d(String name) {
+    m_name = name;
+  }
+
+  @Override
+  public void close() {
+    if (m_entry != null) {
+      m_entry.close();
+    }
+  }
+
+  /**
+   * Set the pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  public synchronized void setPose(Pose2d pose) {
+    setPoses(pose);
+  }
+
+  /**
+   * Set the pose from x, y, and rotation.
+   *
+   * @param x X location, in meters
+   * @param y Y location, in meters
+   * @param rotation rotation
+   */
+  public synchronized void setPose(double x, double y, Rotation2d rotation) {
+    setPose(new Pose2d(x, y, rotation));
+  }
+
+  /**
+   * Set the pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  public synchronized void setPose(Distance x, Distance y, Rotation2d rotation) {
+    setPose(new Pose2d(x.in(Meters), y.in(Meters), rotation));
+  }
+
+  /**
+   * Get the pose.
+   *
+   * @return 2D pose
+   */
+  public synchronized Pose2d getPose() {
+    updateFromEntry();
+    if (m_poses.isEmpty()) {
+      return Pose2d.kZero;
+    }
+    return m_poses.get(0);
+  }
+
+  /**
+   * Set multiple poses from a list of Pose objects. The total number of poses is limited to 85.
+   *
+   * @param poses list of 2D poses
+   */
+  public synchronized void setPoses(List<Pose2d> poses) {
+    m_poses.clear();
+    m_poses.addAll(poses);
+    updateEntry();
+  }
+
+  /**
+   * Set multiple poses from a list of Pose objects. The total number of poses is limited to 85.
+   *
+   * @param poses list of 2D poses
+   */
+  public synchronized void setPoses(Pose2d... poses) {
+    m_poses.clear();
+    Collections.addAll(m_poses, poses);
+    updateEntry();
+  }
+
+  /**
+   * Sets poses from a trajectory.
+   *
+   * @param trajectory The trajectory from which the poses should be added.
+   */
+  public synchronized void setTrajectory(Trajectory trajectory) {
+    m_poses.clear();
+    for (Trajectory.State state : trajectory.getStates()) {
+      m_poses.add(state.pose);
+    }
+    updateEntry();
+  }
+
+  /**
+   * Get multiple poses.
+   *
+   * @return list of 2D poses
+   */
+  public synchronized List<Pose2d> getPoses() {
+    updateFromEntry();
+    return new ArrayList<>(m_poses);
+  }
+
+  void updateEntry() {
+    updateEntry(false);
+  }
+
+  synchronized void updateEntry(boolean setDefault) {
+    if (m_entry == null) {
+      return;
+    }
+
+    double[] arr = new double[m_poses.size() * 3];
+    int ndx = 0;
+    for (Pose2d pose : m_poses) {
+      Translation2d translation = pose.getTranslation();
+      arr[ndx + 0] = translation.getX();
+      arr[ndx + 1] = translation.getY();
+      arr[ndx + 2] = pose.getRotation().getDegrees();
+      ndx += 3;
+    }
+
+    if (setDefault) {
+      m_entry.setDefault(arr);
+    } else {
+      m_entry.set(arr);
+    }
+  }
+
+  private synchronized void updateFromEntry() {
+    if (m_entry == null) {
+      return;
+    }
+
+    double[] arr = m_entry.get(null);
+    if (arr != null) {
+      if ((arr.length % 3) != 0) {
+        return;
+      }
+
+      m_poses.clear();
+      for (int i = 0; i < arr.length; i += 3) {
+        m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
+      }
+    }
+  }
+
+  String m_name;
+  DoubleArrayEntry m_entry;
+  private final List<Pose2d> m_poses = new ArrayList<>();
+}

wpilibj/src/main/java/org/wpilib/smartdashboard/ListenerExecutor.java

@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.concurrent.Executor;
+
+/**
+ * An executor for running listener tasks posted by {@link edu.wpi.first.wpilibj.Sendable} listeners
+ * synchronously from the main application thread.
+ */
+class ListenerExecutor implements Executor {
+  private final Collection<Runnable> m_tasks = new ArrayList<>();
+  private final Object m_lock = new Object();
+
+  /**
+   * Posts a task to the executor to be run synchronously from the main thread.
+   *
+   * @param task The task to run synchronously from the main thread.
+   */
+  @Override
+  public void execute(Runnable task) {
+    synchronized (m_lock) {
+      m_tasks.add(task);
+    }
+  }
+
+  /** Runs all posted tasks. Called periodically from main thread. */
+  public void runListenerTasks() {
+    // Locally copy tasks from internal list; minimizes blocking time
+    Collection<Runnable> tasks;
+    synchronized (m_lock) {
+      tasks = new ArrayList<>(m_tasks);
+      m_tasks.clear();
+    }
+
+    // Run all tasks
+    for (Runnable task : tasks) {
+      task.run();
+    }
+  }
+}