SCRIPT Move java files

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;
+  }
+}