SCRIPT Move cc files

wpilibc/src/main/native/include/wpi/ExpansionHub.hpp

@@ -0,0 +1,92 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string_view>
+
+#include <wpi/mutex.h>
+
+namespace frc {
+class ExpansionHubServo;
+class ExpansionHubMotor;
+
+/** This class controls a REV ExpansionHub plugged in over USB to Systemcore. */
+class ExpansionHub {
+ public:
+  /**
+   * Constructs a new ExpansionHub for a given USB ID
+   *
+   * Multiple instances can be constructed, but will point to the same backing
+   * object with a ref count.
+   *
+   * @param usbId The USB Port ID the hub is plugged into.
+   */
+  explicit ExpansionHub(int usbId);
+  ~ExpansionHub() noexcept = default;
+
+  friend class ExpansionHubServo;
+  friend class ExpansionHubMotor;
+
+  /**
+   * Constructs a servo at the requested channel on this hub.
+   *
+   * Only a single instance of each servo per hub can be constructed at a time.
+   *
+   * @param channel The servo channel
+   * @return Servo object
+   */
+  ExpansionHubServo MakeServo(int channel);
+
+  /**
+   * Constructs a motor at the requested channel on this hub.
+   *
+   * Only a single instance of each motor per hub can be constructed at a time.
+   *
+   * @param channel The motor channel
+   * @return Motor object
+   */
+  ExpansionHubMotor MakeMotor(int channel);
+
+  /**
+   * Gets if the hub is currently connected over USB.
+   *
+   * @return True if hub connection, otherwise false
+   */
+  bool IsHubConnected() const;
+
+  /**
+   * Gets the USB ID of this hub.
+   *
+   * @return The USB ID
+   */
+  int GetUsbId() const { return m_usbId; }
+
+  static constexpr int NumUsbPorts = 4;
+  static constexpr int NumServoPorts = 6;
+  static constexpr int NumMotorPorts = 4;
+
+ private:
+  bool CheckAndReserveServo(int channel);
+  void UnreserveServo(int channel);
+
+  bool CheckAndReserveMotor(int channel);
+  void UnreserveMotor(int channel);
+
+  void ReportUsage(std::string_view device, std::string_view data);
+
+  class DataStore;
+  friend class DataStore;
+
+  std::shared_ptr<DataStore> m_dataStore;
+  int m_usbId;
+
+  static wpi::mutex m_handleLock;
+  static std::weak_ptr<DataStore> m_storeMap[4];
+
+  static std::shared_ptr<DataStore> GetForUsbId(int usbId);
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/ExpansionHubMotor.hpp

@@ -0,0 +1,174 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <networktables/BooleanTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/IntegerTopic.h>
+#include <units/angle.h>
+#include <units/current.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+#include "frc/ExpansionHub.h"
+#include "frc/ExpansionHubPidConstants.h"
+
+namespace frc {
+
+/** This class controls a specific motor and encoder hooked up to an
+ * ExpansionHub. */
+class ExpansionHubMotor {
+ public:
+  /**
+   * Constructs a servo at the requested channel on a specific USB port.
+   *
+   * @param usbId The USB port ID the hub is connected to
+   * @param channel The motor channel
+   */
+  ExpansionHubMotor(int usbId, int channel);
+  ~ExpansionHubMotor() noexcept;
+
+  /**
+   * Sets the percentage power to run the motor at, between -1 and 1.
+   *
+   * @param power The power to drive the motor at
+   */
+  void SetPercentagePower(double power);
+
+  /**
+   * Sets the voltage to run the motor at. This value will be continously scaled
+   * to match the input voltage.
+   *
+   * @param voltage The voltage to drive the motor at
+   */
+  void SetVoltage(units::volt_t voltage);
+
+  /**
+   * Command the motor to drive to a specific position setpoint. This value will
+   * be scaled by SetDistancePerCount and influenced by the PID constants.
+   *
+   * @param setpoint The position setpoint to drive the motor to
+   */
+  void SetPositionSetpoint(double setpoint);
+
+  /**
+   * Command the motor to drive to a specific velocity setpoint. This value will
+   * be scaled by SetDistancePerCount and influenced by the PID constants.
+   *
+   * @param setpoint The velocity setpoint to drive the motor to
+   */
+  void SetVelocitySetpoint(double setpoint);
+
+  /**
+   * Sets if the motor output is enabled or not. Defaults to false.
+   *
+   * @param enabled True to enable, false to disable
+   */
+  void SetEnabled(bool enabled);
+
+  /**
+   * Sets if the motor should float or brake when 0 is commanded. Defaults to
+   * false.
+   *
+   * @param floatOn0 True to float when commanded 0, false to brake
+   */
+  void SetFloatOn0(bool floatOn0);
+
+  /**
+   * Gets the current being pulled by the motor.
+   *
+   * @return Motor current
+   */
+  units::ampere_t GetCurrent() const;
+
+  /**
+   * Sets the distance per count of the encoder. Used to scale encoder readings.
+   *
+   * @param perCount The distance moved per count of the encoder
+   */
+  void SetDistancePerCount(double perCount);
+
+  /**
+   * Gets the current velocity of the motor encoder. Scaled into
+   * distancePerCount units.
+   *
+   * @return Encoder velocity
+   */
+  double GetEncoderVelocity() const;
+
+  /**
+   * Gets the current position of the motor encoder. Scaled into
+   * distancePerCount units.
+   *
+   * @return Encoder position
+   */
+  double GetEncoderPosition() const;
+
+  /**
+   * Sets if the motor and encoder should be reversed.
+   *
+   * @param reversed True to reverse encoder, false otherwise
+   */
+  void SetReversed(bool reversed);
+
+  /** Reset the encoder count to 0. */
+  void ResetEncoder();
+
+  /**
+   * Gets the PID constants object for velocity PID.
+   *
+   * @return Velocity PID constants object
+   */
+  ExpansionHubPidConstants& GetVelocityPidConstants();
+
+  /**
+   * Gets the PID constants object for position PID.
+   *
+   * @return Position PID constants object
+   */
+  ExpansionHubPidConstants& GetPositionPidConstants();
+
+  /**
+   * Gets if the underlying ExpansionHub is connected.
+   *
+   * @return True if hub is connected, otherwise false
+   */
+  bool IsHubConnected() { return m_hub.IsHubConnected(); }
+
+  /**
+   * Sets this motor to follow another motor on the same hub.
+   *
+   * This does not support following motors that are also followers.
+   * Additionally, the direction of both motors will be the same.
+   *
+   * @param leader The motor to follow
+   */
+  void Follow(const ExpansionHubMotor& leader);
+
+ private:
+  ExpansionHub m_hub;
+  int m_channel;
+
+  nt::DoubleSubscriber m_encoderSubscriber;
+  nt::DoubleSubscriber m_encoderVelocitySubscriber;
+  nt::DoubleSubscriber m_currentSubscriber;
+
+  nt::DoublePublisher m_setpointPublisher;
+  nt::BooleanPublisher m_floatOn0Publisher;
+  nt::BooleanPublisher m_enabledPublisher;
+
+  nt::IntegerPublisher m_modePublisher;
+
+  nt::BooleanPublisher m_reversedPublisher;
+  nt::BooleanPublisher m_resetEncoderPublisher;
+
+  nt::DoublePublisher m_distancePerCountPublisher;
+
+  ExpansionHubPidConstants m_velocityPidConstants;
+  ExpansionHubPidConstants m_positionPidConstants;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/ExpansionHubPidConstants.hpp

@@ -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.
+
+#pragma once
+
+#include <networktables/BooleanTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/IntegerTopic.h>
+
+namespace frc {
+class ExpansionHubMotor;
+
+/** This class contains PID constants for an ExpansionHub motor. */
+class ExpansionHubPidConstants {
+ public:
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * Sets the proportional, integral, and differential coefficients.
+   *
+   * @param p The proportional coefficient. Must be >= 0.
+   * @param i The integral coefficient. Must be >= 0.
+   * @param d The differential coefficient. Must be >= 0.
+   */
+  void SetPID(double p, double i, double d);
+
+  /**
+   * Sets the feed forward gains to the specified values.
+   *
+   * The units should be radians for angular systems and meters for linear
+   * systems.
+   *
+   * The PID control period is 10ms
+   *
+   * @param s The static gain in volts.
+   * @param v The velocity gain in V/(units/s).
+   * @param a The acceleration gain in V/(units/s²).
+   */
+  void SetFF(double s, double v, double a);
+
+  /**
+   * Enables continuous input.
+   *
+   * Rather then using the max and min input range as constraints, it considers
+   * them to be the same point and automatically calculates the shortest route
+   * to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  void EnableContinousInput(double minimumInput, double maximumInput);
+
+  /**
+   * Disables continuous input.
+   */
+  void DisableContinousInput();
+
+  ExpansionHubPidConstants(ExpansionHubPidConstants&) = delete;
+  ExpansionHubPidConstants& operator=(ExpansionHubPidConstants&) = delete;
+
+  ExpansionHubPidConstants(ExpansionHubPidConstants&&) = default;
+  ExpansionHubPidConstants& operator=(ExpansionHubPidConstants&&) = default;
+
+  friend class ExpansionHubMotor;
+
+ private:
+  ExpansionHubPidConstants(int usbId, int channel, bool isVelocityPid);
+
+  nt::DoublePublisher m_pPublisher;
+  nt::DoublePublisher m_iPublisher;
+  nt::DoublePublisher m_dPublisher;
+  nt::DoublePublisher m_sPublisher;
+  nt::DoublePublisher m_vPublisher;
+  nt::DoublePublisher m_aPublisher;
+
+  nt::BooleanPublisher m_continuousPublisher;
+  nt::DoublePublisher m_continuousMinimumPublisher;
+  nt::DoublePublisher m_continuousMaximumPublisher;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/ExpansionHubServo.hpp

@@ -0,0 +1,140 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <networktables/BooleanTopic.h>
+#include <networktables/IntegerTopic.h>
+#include <units/angle.h>
+#include <units/time.h>
+
+#include "frc/ExpansionHub.h"
+
+namespace frc {
+
+/** This class controls a specific servo hooked up to an ExpansionHub. */
+class ExpansionHubServo {
+ public:
+  /**
+   * Constructs a servo at the requested channel on a specific USB port.
+   *
+   * @param usbId The USB port ID the hub is connected to
+   * @param channel The servo channel
+   */
+  ExpansionHubServo(int usbId, int channel);
+  ~ExpansionHubServo() noexcept;
+
+  /**
+   * Set the servo position.
+   *
+   * Servo values range from 0.0 to 1.0 corresponding to the range of full left
+   * to full right.
+   *
+   * @param value Position from 0.0 to 1.0.
+   */
+  void Set(double value);
+
+  /**
+   * Sets the servo angle
+   *
+   * Servo angles range from 0 to 180 degrees. Use Set() with your own scaler
+   * for other angle ranges.
+   *
+   * @param angle Position in angle units. Will be scaled between 0 and 180
+   * degrees
+   */
+  void SetAngle(units::degree_t angle);
+
+  /**
+   * Sets the raw pulse width output on the servo.
+   *
+   * @param pulseWidth Pulse width
+   */
+  void SetPulseWidth(units::microsecond_t pulseWidth);
+
+  /**
+   * Sets if the servo output is enabled or not. Defaults to false.
+   *
+   * @param enabled True to enable, false to disable
+   */
+  void SetEnabled(bool enabled);
+
+  /**
+   * Sets the frame period for the servo. Defaults to 20ms.
+   *
+   * @param framePeriod The frame period
+   */
+  void SetFramePeriod(units::microsecond_t framePeriod);
+
+  /**
+   * Gets if the underlying ExpansionHub is connected.
+   *
+   * @return True if hub is connected, otherwise false
+   */
+  bool IsHubConnected() const { return m_hub.IsHubConnected(); }
+
+  /**
+   * Sets the angle range for the setAngle call.
+   * By default, this is 0 to 180 degrees.
+   *
+   * Maximum angle must be greater than minimum angle.
+   *
+   * @param minAngle Minimum angle
+   * @param maxAngle Maximum angle
+   */
+  void SetAngleRange(units::degree_t minAngle, units::degree_t maxAngle);
+
+  /**
+   * Sets the PWM range for the servo.
+   * By default, this is 600 to 2400 microseconds.
+   *
+   * Maximum must be greater than minimum.
+   *
+   * @param minPwm Minimum PWM
+   * @param maxPwm Maximum PWM
+   */
+  void SetPWMRange(units::microsecond_t minPwm, units::microsecond_t maxPwm);
+
+  /**
+   * Sets whether the servo is reversed.
+   *
+   * This will reverse both Set() and SetAngle().
+   *
+   * @param reversed True to reverse, false for normal
+   */
+  void SetReversed(bool reversed);
+
+  /**
+   * Enables or disables continuous rotation mode.
+   *
+   * In continuous rotation mode, the servo will interpret
+   * Set() commands to between -1.0 and 1.0, instead of 0.0 to 1.0.
+   *
+   * @param enable True to enable continuous rotation mode, false to disable
+   */
+  void SetContinousRotationMode(bool enable);
+
+ private:
+  units::microsecond_t GetFullRangeScaleFactor();
+  units::degree_t GetServoAngleRange();
+
+  ExpansionHub m_hub;
+  int m_channel;
+
+  units::degree_t m_maxServoAngle = 180.0_deg;
+  units::degree_t m_minServoAngle = 0.0_deg;
+
+  units::microsecond_t m_minPwm = 600_us;
+  units::microsecond_t m_maxPwm = 2400_us;
+
+  bool m_reversed = false;
+  bool m_continousMode = false;
+
+  nt::IntegerPublisher m_pulseWidthPublisher;
+  nt::IntegerPublisher m_framePeriodPublisher;
+  nt::BooleanPublisher m_enabledPublisher;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/counter/EdgeConfiguration.hpp

@@ -0,0 +1,17 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace frc {
+/**
+ * Edge configuration.
+ */
+enum class EdgeConfiguration {
+  /// Rising edge configuration.
+  kRisingEdge = 0,
+  /// Falling edge configuration.
+  kFallingEdge = 1,
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/counter/Tachometer.hpp

@@ -0,0 +1,120 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Counter.h>
+#include <hal/Types.h>
+#include <units/angular_velocity.h>
+#include <units/frequency.h>
+#include <units/time.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "EdgeConfiguration.h"
+
+namespace frc {
+/**
+ * Tachometer for getting rotational speed from a device.
+ *
+ * <p>The Tachometer class measures the time between digital pulses to
+ * determine the rotation speed of a mechanism. Examples of devices that could
+ * be used with the tachometer class are a hall effect sensor, break beam
+ * sensor, or optical sensor detecting tape on a shooter wheel. Unlike
+ * encoders, this class only needs a single digital input.
+ */
+class Tachometer : public wpi::Sendable,
+                   public wpi::SendableHelper<Tachometer> {
+ public:
+  /**
+   * Constructs a new tachometer.
+   *
+   * @param channel The DIO Channel.
+   * @param configuration Edge configuration
+   */
+  Tachometer(int channel, EdgeConfiguration configuration);
+
+  Tachometer(Tachometer&&) = default;
+  Tachometer& operator=(Tachometer&&) = default;
+
+  ~Tachometer() override = default;
+
+  /**
+   * Sets the configuration for the channel.
+   *
+   * @param configuration The channel configuration.
+   */
+  void SetEdgeConfiguration(EdgeConfiguration configuration);
+
+  /**
+   * Gets the tachometer frequency.
+   *
+   * @return Current frequency.
+   */
+  units::hertz_t GetFrequency() const;
+
+  /**
+   * Gets the tachometer period.
+   *
+   * @return Current period.
+   */
+  units::second_t GetPeriod() const;
+
+  /**
+   * Gets the number of edges per revolution.
+   *
+   * @return Edges per revolution.
+   */
+  int GetEdgesPerRevolution() const;
+
+  /**
+   * Sets the number of edges per revolution.
+   *
+   * @param edges Edges per revolution.
+   */
+  void SetEdgesPerRevolution(int edges);
+
+  /**
+   * Gets the current tachometer revolutions per second.
+   *
+   * SetEdgesPerRevolution must be set with a non 0 value for this to work.
+   *
+   * @return Current RPS.
+   */
+  units::turns_per_second_t GetRevolutionsPerSecond() const;
+
+  /**
+   * Gets the current tachometer revolutions per minute.
+   *
+   * SetEdgesPerRevolution must be set with a non 0 value for this to work.
+   *
+   * @return Current RPM.
+   */
+  units::revolutions_per_minute_t GetRevolutionsPerMinute() const;
+
+  /**
+   * Gets if the tachometer is stopped.
+   *
+   * @return True if the tachometer is stopped.
+   */
+  bool GetStopped() const;
+
+  /**
+   * Sets the maximum period before the tachometer is considered stopped.
+   *
+   * @param maxPeriod The max period.
+   */
+  void SetMaxPeriod(units::second_t maxPeriod);
+
+ protected:
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  hal::Handle<HAL_CounterHandle, HAL_FreeCounter> m_handle;
+  int m_edgesPerRevolution;
+  int32_t m_channel;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/counter/UpDownCounter.hpp

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Counter.h>
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "EdgeConfiguration.h"
+
+namespace frc {
+/** Up Down Counter.
+ *
+ * This class can count edges on a single digital input or count up based on an
+ * edge from one digital input and down on an edge from another digital input.
+ *
+ */
+class UpDownCounter : public wpi::Sendable,
+                      public wpi::SendableHelper<UpDownCounter> {
+ public:
+  /**
+   * Constructs a new UpDown Counter.
+   *
+   * @param channel The DIO channel
+   * @param configuration Edge configuration
+   */
+  UpDownCounter(int channel, EdgeConfiguration configuration);
+
+  UpDownCounter(UpDownCounter&&) = default;
+  UpDownCounter& operator=(UpDownCounter&&) = default;
+
+  ~UpDownCounter() override = default;
+
+  /**
+   * Gets the current count.
+   *
+   * @return The current count.
+   */
+  int GetCount() const;
+
+  /** Resets the current count. */
+  void Reset();
+
+  /**
+   * Sets the configuration for the channel.
+   *
+   * @param configuration The channel configuration.
+   */
+  void SetEdgeConfiguration(EdgeConfiguration configuration);
+
+ protected:
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  hal::Handle<HAL_CounterHandle, HAL_FreeCounter> m_handle;
+  int32_t m_channel;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/drive/DifferentialDrive.hpp

@@ -0,0 +1,212 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <string>
+
+#include <wpi/deprecated.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/drive/RobotDriveBase.h"
+
+namespace frc {
+
+class MotorController;
+
+/**
+ * A class for driving differential drive/skid-steer drive platforms such as
+ * the Kit of Parts drive base, "tank drive", or West Coast Drive.
+ *
+ * These drive bases typically have drop-center / skid-steer with two or more
+ * wheels per side (e.g., 6WD or 8WD). This class takes a setter per side. For
+ * four and six motor drivetrains, use CAN motor controller followers or
+ * PWMMotorController::AddFollower().
+ *
+ * A differential drive robot has left and right wheels separated by an
+ * arbitrary width.
+ *
+ * Drive base diagram:
+ * <pre>
+ * |_______|
+ * | |   | |
+ *   |   |
+ * |_|___|_|
+ * |       |
+ * </pre>
+ *
+ * Each drive function provides different inverse kinematic relations for a
+ * differential drive robot.
+ *
+ * This library uses the NWU axes convention (North-West-Up as external
+ * reference in the world frame). The positive X axis points ahead, the positive
+ * Y axis points to the left, and the positive Z axis points up. Rotations
+ * follow the right-hand rule, so counterclockwise rotation around the Z axis is
+ * positive.
+ *
+ * Inputs smaller then 0.02 will be set to 0, and larger values will be scaled
+ * so that the full range is still used. This deadband value can be changed
+ * with SetDeadband().
+ *
+ * MotorSafety is enabled by default. The tankDrive, arcadeDrive,
+ * or curvatureDrive methods should be called periodically to avoid Motor
+ * Safety timeouts.
+ */
+class DifferentialDrive : public RobotDriveBase,
+                          public wpi::Sendable,
+                          public wpi::SendableHelper<DifferentialDrive> {
+ public:
+  /**
+   * Wheel speeds for a differential drive.
+   *
+   * Uses normalized voltage [-1.0..1.0].
+   */
+  struct WheelSpeeds {
+    /// Left wheel speed.
+    double left = 0.0;
+    /// Right wheel speed.
+    double right = 0.0;
+  };
+
+  WPI_IGNORE_DEPRECATED
+
+  /**
+   * Construct a DifferentialDrive.
+   *
+   * To pass multiple motors per side, use CAN motor controller followers or
+   * PWMSpeedController::AddFollower(). If a motor needs to be inverted, do so
+   * before passing it in.
+   *
+   * @param leftMotor Left motor.
+   * @param rightMotor Right motor.
+   */
+  DifferentialDrive(MotorController& leftMotor, MotorController& rightMotor);
+
+  WPI_UNIGNORE_DEPRECATED
+
+  /**
+   * Construct a DifferentialDrive.
+   *
+   * To pass multiple motors per side, use CAN motor controller followers or
+   * PWMSpeedController::AddFollower(). If a motor needs to be inverted, do so
+   * before passing it in.
+   *
+   * @param leftMotor Left motor setter.
+   * @param rightMotor Right motor setter.
+   */
+  DifferentialDrive(std::function<void(double)> leftMotor,
+                    std::function<void(double)> rightMotor);
+
+  ~DifferentialDrive() override = default;
+
+  DifferentialDrive(DifferentialDrive&&) = default;
+  DifferentialDrive& operator=(DifferentialDrive&&) = default;
+
+  /**
+   * Arcade drive method for differential drive platform.
+   *
+   * Note: Some drivers may prefer inverted rotation controls. This can be done
+   * by negating the value passed for rotation.
+   *
+   * @param xSpeed        The speed at which the robot should drive along the X
+   *                      axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation     The rotation rate of the robot around the Z axis
+   *                      [-1.0..1.0]. Counterclockwise is positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   */
+  void ArcadeDrive(double xSpeed, double zRotation, bool squareInputs = true);
+
+  /**
+   * Curvature drive method for differential drive platform.
+   *
+   * The rotation argument controls the curvature of the robot's path rather
+   * than its rate of heading change. This makes the robot more controllable at
+   * high speeds.
+   *
+   * @param xSpeed           The robot's speed along the X axis [-1.0..1.0].
+   *                         Forward is positive.
+   * @param zRotation        The normalized curvature [-1.0..1.0].
+   *                         Counterclockwise is positive.
+   * @param allowTurnInPlace If set, overrides constant-curvature turning for
+   *                         turn-in-place maneuvers. zRotation will control
+   *                         turning rate instead of curvature.
+   */
+  void CurvatureDrive(double xSpeed, double zRotation, bool allowTurnInPlace);
+
+  /**
+   * Tank drive method for differential drive platform.
+   *
+   * @param leftSpeed     The robot left side's speed along the X axis
+   *                      [-1.0..1.0]. Forward is positive.
+   * @param rightSpeed    The robot right side's speed along the X axis
+   *                      [-1.0..1.0]. Forward is positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   */
+  void TankDrive(double leftSpeed, double rightSpeed, bool squareInputs = true);
+
+  /**
+   * Arcade drive inverse kinematics for differential drive platform.
+   *
+   * Note: Some drivers may prefer inverted rotation controls. This can be done
+   * by negating the value passed for rotation.
+   *
+   * @param xSpeed       The speed at which the robot should drive along the X
+   *                     axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation    The rotation rate of the robot around the Z axis
+   *                     [-1.0..1.0]. Clockwise is positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   * @return Wheel speeds [-1.0..1.0].
+   */
+  static WheelSpeeds ArcadeDriveIK(double xSpeed, double zRotation,
+                                   bool squareInputs = true);
+
+  /**
+   * Curvature drive inverse kinematics for differential drive platform.
+   *
+   * The rotation argument controls the curvature of the robot's path rather
+   * than its rate of heading change. This makes the robot more controllable at
+   * high speeds.
+   *
+   * @param xSpeed           The robot's speed along the X axis [-1.0..1.0].
+   *                         Forward is positive.
+   * @param zRotation        The normalized curvature [-1.0..1.0]. Clockwise is
+   *                         positive.
+   * @param allowTurnInPlace If set, overrides constant-curvature turning for
+   *                         turn-in-place maneuvers. zRotation will control
+   *                         turning rate instead of curvature.
+   * @return Wheel speeds [-1.0..1.0].
+   */
+  static WheelSpeeds CurvatureDriveIK(double xSpeed, double zRotation,
+                                      bool allowTurnInPlace);
+
+  /**
+   * Tank drive inverse kinematics for differential drive platform.
+   *
+   * @param leftSpeed    The robot left side's speed along the X axis
+   *                     [-1.0..1.0]. Forward is positive.
+   * @param rightSpeed   The robot right side's speed along the X axis
+   *                     [-1.0..1.0]. Forward is positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   * @return Wheel speeds [-1.0..1.0].
+   */
+  static WheelSpeeds TankDriveIK(double leftSpeed, double rightSpeed,
+                                 bool squareInputs = true);
+
+  void StopMotor() override;
+  std::string GetDescription() const override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  std::function<void(double)> m_leftMotor;
+  std::function<void(double)> m_rightMotor;
+
+  // Used for Sendable property getters
+  double m_leftOutput = 0.0;
+  double m_rightOutput = 0.0;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/drive/MecanumDrive.hpp

@@ -0,0 +1,185 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <units/angle.h>
+#include <wpi/deprecated.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/drive/RobotDriveBase.h"
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+class MotorController;
+
+/**
+ * A class for driving Mecanum drive platforms.
+ *
+ * Mecanum drives are rectangular with one wheel on each corner. Each wheel has
+ * rollers toed in 45 degrees toward the front or back. When looking at the
+ * wheels from the top, the roller axles should form an X across the robot.
+ *
+ * Drive base diagram:
+ * <pre>
+ * \\_______/
+ * \\ |   | /
+ *   |   |
+ * /_|___|_\\
+ * /       \\
+ * </pre>
+ *
+ * Each Drive() function provides different inverse kinematic relations for a
+ * Mecanum drive robot.
+ *
+ * This library uses the NWU axes convention (North-West-Up as external
+ * reference in the world frame). The positive X axis points ahead, the positive
+ * Y axis points to the left, and the positive Z axis points up. Rotations
+ * follow the right-hand rule, so counterclockwise rotation around the Z axis is
+ * positive.
+ *
+ * Inputs smaller then 0.02 will be set to 0, and larger values will be scaled
+ * so that the full range is still used. This deadband value can be changed
+ * with SetDeadband().
+ *
+ * MotorSafety is enabled by default. The DriveCartesian or DrivePolar
+ * methods should be called periodically to avoid Motor Safety timeouts.
+ */
+class MecanumDrive : public RobotDriveBase,
+                     public wpi::Sendable,
+                     public wpi::SendableHelper<MecanumDrive> {
+ public:
+  /**
+   * Wheel speeds for a mecanum drive.
+   *
+   * Uses normalized voltage [-1.0..1.0].
+   */
+  struct WheelSpeeds {
+    /// Front-left wheel speed.
+    double frontLeft = 0.0;
+    /// Front-right wheel speed.
+    double frontRight = 0.0;
+    /// Rear-left wheel speed.
+    double rearLeft = 0.0;
+    /// Rear-right wheel speed.
+    double rearRight = 0.0;
+  };
+
+  WPI_IGNORE_DEPRECATED
+
+  /**
+   * Construct a MecanumDrive.
+   *
+   * If a motor needs to be inverted, do so before passing it in.
+   *
+   * @param frontLeftMotor Front-left motor.
+   * @param rearLeftMotor Rear-left motor.
+   * @param frontRightMotor Front-right motor.
+   * @param rearRightMotor Rear-right motor.
+   */
+  MecanumDrive(MotorController& frontLeftMotor, MotorController& rearLeftMotor,
+               MotorController& frontRightMotor,
+               MotorController& rearRightMotor);
+
+  WPI_UNIGNORE_DEPRECATED
+
+  /**
+   * Construct a MecanumDrive.
+   *
+   * If a motor needs to be inverted, do so before passing it in.
+   *
+   * @param frontLeftMotor Front-left motor setter.
+   * @param rearLeftMotor Rear-left motor setter.
+   * @param frontRightMotor Front-right motor setter.
+   * @param rearRightMotor Rear-right motor setter.
+   */
+  MecanumDrive(std::function<void(double)> frontLeftMotor,
+               std::function<void(double)> rearLeftMotor,
+               std::function<void(double)> frontRightMotor,
+               std::function<void(double)> rearRightMotor);
+
+  ~MecanumDrive() override = default;
+
+  MecanumDrive(MecanumDrive&&) = default;
+  MecanumDrive& operator=(MecanumDrive&&) = default;
+
+  /**
+   * Drive method for Mecanum platform.
+   *
+   * Angles are measured counterclockwise from the positive X axis. The robot's
+   * speed is independent from its angle or rotation rate.
+   *
+   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is
+   *                  positive.
+   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Left is
+   *                  positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
+   *                  Counterclockwise is positive.
+   * @param gyroAngle The gyro heading around the Z axis. Use this to implement
+   *                  field-oriented controls.
+   */
+  void DriveCartesian(double xSpeed, double ySpeed, double zRotation,
+                      Rotation2d gyroAngle = 0_rad);
+
+  /**
+   * Drive method for Mecanum platform.
+   *
+   * Angles are measured counterclockwise from the positive X axis. The robot's
+   * speed is independent from its angle or rotation rate.
+   *
+   * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is
+   *                  positive.
+   * @param angle     The angle around the Z axis at which the robot drives.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
+   *                  Counterclockwise is positive.
+   */
+  void DrivePolar(double magnitude, Rotation2d angle, double zRotation);
+
+  /**
+   * Cartesian inverse kinematics for Mecanum platform.
+   *
+   * Angles are measured counterclockwise from the positive X axis. The robot's
+   * speed is independent from its angle or rotation rate.
+   *
+   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is
+   *                  positive.
+   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Left is
+   *                  positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
+   *                  Counterclockwise is positive.
+   * @param gyroAngle The gyro heading around the Z axis. Use this to implement
+   *                  field-oriented controls.
+   * @return Wheel speeds [-1.0..1.0].
+   */
+  static WheelSpeeds DriveCartesianIK(double xSpeed, double ySpeed,
+                                      double zRotation,
+                                      Rotation2d gyroAngle = 0_rad);
+
+  void StopMotor() override;
+  std::string GetDescription() const override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  std::function<void(double)> m_frontLeftMotor;
+  std::function<void(double)> m_rearLeftMotor;
+  std::function<void(double)> m_frontRightMotor;
+  std::function<void(double)> m_rearRightMotor;
+
+  // Used for Sendable property getters
+  double m_frontLeftOutput = 0.0;
+  double m_rearLeftOutput = 0.0;
+  double m_frontRightOutput = 0.0;
+  double m_rearRightOutput = 0.0;
+
+  bool reported = false;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/drive/RobotDriveBase.hpp

@@ -0,0 +1,99 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <span>
+#include <string>
+
+#include "frc/MotorSafety.h"
+
+namespace frc {
+
+/**
+ * Common base class for drive platforms.
+ *
+ * MotorSafety is enabled by default.
+ */
+class RobotDriveBase : public MotorSafety {
+ public:
+  /**
+   * The location of a motor on the robot for the purpose of driving.
+   */
+  enum MotorType {
+    /// Front-left motor.
+    kFrontLeft = 0,
+    /// Front-right motor.
+    kFrontRight = 1,
+    /// Rear-left motor.
+    kRearLeft = 2,
+    /// Rear-right motor.
+    kRearRight = 3,
+    /// Left motor.
+    kLeft = 0,
+    /// Right motor.
+    kRight = 1,
+    /// Back motor.
+    kBack = 2
+  };
+
+  RobotDriveBase();
+  ~RobotDriveBase() override = default;
+
+  RobotDriveBase(RobotDriveBase&&) = default;
+  RobotDriveBase& operator=(RobotDriveBase&&) = default;
+
+  /**
+   * Sets the deadband applied to the drive inputs (e.g., joystick values).
+   *
+   * The default value is 0.02. Inputs smaller than the deadband are set to 0.0
+   * while inputs larger than the deadband are scaled from 0.0 to 1.0. See
+   * frc::ApplyDeadband().
+   *
+   * @param deadband The deadband to set.
+   */
+  void SetDeadband(double deadband);
+
+  /**
+   * Configure the scaling factor for using RobotDrive with motor controllers in
+   * a mode other than PercentVbus or to limit the maximum output.
+   *
+   * @param maxOutput Multiplied with the output percentage computed by the
+   *                  drive functions.
+   */
+  void SetMaxOutput(double maxOutput);
+
+  /**
+   * Feed the motor safety object. Resets the timer that will stop the motors if
+   * it completes.
+   *
+   * @see MotorSafetyHelper::Feed()
+   */
+  void FeedWatchdog();
+
+  void StopMotor() override = 0;
+  std::string GetDescription() const override = 0;
+
+ protected:
+  /// Default input deadband.
+  static constexpr double kDefaultDeadband = 0.02;
+
+  /// Default maximum output.
+  static constexpr double kDefaultMaxOutput = 1.0;
+
+  /**
+   * Renormalize all wheel speeds if the magnitude of any wheel is greater than
+   * 1.0.
+   */
+  static void Desaturate(std::span<double> wheelSpeeds);
+
+  /// Input deadband.
+  double m_deadband = kDefaultDeadband;
+
+  /// Maximum output.
+  double m_maxOutput = kDefaultMaxOutput;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/driverstation/DSControlWord.hpp

@@ -0,0 +1,102 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/DriverStationTypes.h>
+
+namespace frc {
+
+/**
+ * A wrapper around Driver Station control word.
+ */
+class DSControlWord {
+ public:
+  /**
+   * DSControlWord constructor.
+   *
+   * Upon construction, the current Driver Station control word is read and
+   * stored internally.
+   */
+  DSControlWord();
+
+  /**
+   * Check if the DS has enabled the robot.
+   *
+   * @return True if the robot is enabled and the DS is connected
+   */
+  bool IsEnabled() const;
+
+  /**
+   * Check if the robot is disabled.
+   *
+   * @return True if the robot is explicitly disabled or the DS is not connected
+   */
+  bool IsDisabled() const;
+
+  /**
+   * Check if the robot is e-stopped.
+   *
+   * @return True if the robot is e-stopped
+   */
+  bool IsEStopped() const;
+
+  /**
+   * Check if the DS is commanding autonomous mode.
+   *
+   * @return True if the robot is being commanded to be in autonomous mode
+   */
+  bool IsAutonomous() const;
+
+  /**
+   * Check if the DS is commanding autonomous mode and if it has enabled the
+   * robot.
+   *
+   * @return True if the robot is being commanded to be in autonomous mode and
+   * enabled.
+   */
+  bool IsAutonomousEnabled() const;
+
+  /**
+   * Check if the DS is commanding teleop mode.
+   *
+   * @return True if the robot is being commanded to be in teleop mode
+   */
+  bool IsTeleop() const;
+
+  /**
+   * Check if the DS is commanding teleop mode and if it has enabled the robot.
+   *
+   * @return True if the robot is being commanded to be in teleop mode and
+   * enabled.
+   */
+  bool IsTeleopEnabled() const;
+
+  /**
+   * Check if the DS is commanding test mode.
+   *
+   * @return True if the robot is being commanded to be in test mode
+   */
+  bool IsTest() const;
+
+  /**
+   * Check if the DS is attached.
+   *
+   * @return True if the DS is connected to the robot
+   */
+  bool IsDSAttached() const;
+
+  /**
+   * Is the driver station attached to a Field Management System?
+   *
+   * @return True if the robot is competing on a field being controlled by a
+   *         Field Management System
+   */
+  bool IsFMSAttached() const;
+
+ private:
+  HAL_ControlWord m_controlWord;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp

@@ -0,0 +1,496 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/geometry/Rotation2d.h>
+
+#include <optional>
+#include <string>
+
+#include <hal/DriverStationTypes.h>
+#include <units/time.h>
+#include <wpi/Synchronization.h>
+
+namespace wpi::log {
+class DataLog;
+}  // namespace wpi::log
+
+namespace frc {
+
+/**
+ * Provide access to the network communication data to / from the Driver
+ * Station.
+ */
+class DriverStation final {
+ public:
+  /**
+   * The robot alliance that the robot is a part of.
+   */
+  enum Alliance {
+    /// Red alliance.
+    kRed,
+    /// Blue alliance.
+    kBlue
+  };
+
+  /**
+   * The type of robot match that the robot is part of.
+   */
+  enum MatchType {
+    /// None.
+    kNone,
+    /// Practice.
+    kPractice,
+    /// Qualification.
+    kQualification,
+    /// Elimination.
+    kElimination
+  };
+
+  /**
+   * A controller POV direction.
+   */
+  enum POVDirection : uint8_t {
+    /// POV center.
+    kCenter = HAL_JoystickPOV_kCentered,
+    /// POV up.
+    kUp = HAL_JoystickPOV_kUp,
+    /// POV up right.
+    kUpRight = HAL_JoystickPOV_kRightUp,
+    /// POV right.
+    kRight = HAL_JoystickPOV_kRight,
+    /// POV down right.
+    kDownRight = HAL_JoystickPOV_kRightDown,
+    /// POV down.
+    kDown = HAL_JoystickPOV_kDown,
+    /// POV down left.
+    kDownLeft = HAL_JoystickPOV_kLeftDown,
+    /// POV left.
+    kLeft = HAL_JoystickPOV_kLeft,
+    /// POV up left.
+    kUpLeft = HAL_JoystickPOV_kLeftUp,
+  };
+
+  /**
+   * Gets the angle of a POVDirection.
+   *
+   * @param angle The POVDirection to convert.
+   * @return The angle clockwise from straight up, or std::nullopt if the
+   * POVDirection is kCenter.
+   */
+  static constexpr std::optional<Rotation2d> GetAngle(POVDirection angle) {
+    switch (angle) {
+      case kCenter:
+        return std::nullopt;
+      case kUp:
+        return Rotation2d{0_deg};
+      case kUpRight:
+        return Rotation2d{45_deg};
+      case kRight:
+        return Rotation2d{90_deg};
+      case kDownRight:
+        return Rotation2d{135_deg};
+      case kDown:
+        return Rotation2d{180_deg};
+      case kDownLeft:
+        return Rotation2d{225_deg};
+      case kLeft:
+        return Rotation2d{270_deg};
+      case kUpLeft:
+        return Rotation2d{315_deg};
+      default:
+        return std::nullopt;
+    }
+  }
+
+  /// Number of Joystick ports.
+  static constexpr int kJoystickPorts = 6;
+
+  /**
+   * The state of one joystick button. Button indexes begin at 0.
+   *
+   * @param stick  The joystick to read.
+   * @param button The button index, beginning at 0.
+   * @return The state of the joystick button.
+   */
+  static bool GetStickButton(int stick, int button);
+
+  /**
+   * The state of one joystick button, only if available. Button indexes begin
+   * at 0.
+   *
+   * @param stick  The joystick to read.
+   * @param button The button index, beginning at 0.
+   * @return The state of the joystick button, or empty if unavailable.
+   */
+  static std::optional<bool> GetStickButtonIfAvailable(int stick, int button);
+
+  /**
+   * Whether one joystick button was pressed since the last check. %Button
+   * indexes begin at 1.
+   *
+   * @param stick  The joystick to read.
+   * @param button The button index, beginning at 0.
+   * @return Whether the joystick button was pressed since the last check.
+   */
+  static bool GetStickButtonPressed(int stick, int button);
+
+  /**
+   * Whether one joystick button was released since the last check. %Button
+   * indexes begin at 1.
+   *
+   * @param stick  The joystick to read.
+   * @param button The button index, beginning at 0.
+   * @return Whether the joystick button was released since the last check.
+   */
+  static bool GetStickButtonReleased(int stick, int button);
+
+  /**
+   * Get the value of the axis on a joystick.
+   *
+   * This depends on the mapping of the joystick connected to the specified
+   * port.
+   *
+   * @param stick The joystick to read.
+   * @param axis  The analog axis value to read from the joystick.
+   * @return The value of the axis on the joystick.
+   */
+  static double GetStickAxis(int stick, int axis);
+
+  /**
+   * Get the value of the axis on a joystick, if available.
+   *
+   * This depends on the mapping of the joystick connected to the specified
+   * port.
+   *
+   * @param stick The joystick to read.
+   * @param axis  The analog axis value to read from the joystick.
+   * @return The value of the axis on the joystick, or empty if not available.
+   */
+  static std::optional<double> GetStickAxisIfAvailable(int stick, int axis);
+
+  /**
+   * Get the state of a POV on the joystick.
+   *
+   * @return the angle of the POV.
+   */
+  static POVDirection GetStickPOV(int stick, int pov);
+
+  /**
+   * The state of the buttons on the joystick.
+   *
+   * @param stick The joystick to read.
+   * @return The state of the buttons on the joystick.
+   */
+  static uint64_t GetStickButtons(int stick);
+
+  /**
+   * Returns the maximum axis index on a given joystick port.
+   *
+   * @param stick The joystick port number
+   * @return The maximum axis index on the indicated joystick
+   */
+  static int GetStickAxesMaximumIndex(int stick);
+
+  /**
+   * Returns the mask of available axes on a given joystick port.
+   *
+   * @param stick The joystick port number
+   * @return The mask of available axes on the indicated joystick
+   */
+  static int GetStickAxesAvailable(int stick);
+
+  /**
+   * Returns the maximum POV index on a given joystick port.
+   *
+   * @param stick The joystick port number
+   * @return The maximum POV index on the indicated joystick
+   */
+  static int GetStickPOVsMaximumIndex(int stick);
+
+  /**
+   * Returns the mask of available POVs on a given joystick port.
+   *
+   * @param stick The joystick port number
+   * @return The mask of available POVs on the indicated joystick
+   */
+  static int GetStickPOVsAvailable(int stick);
+
+  /**
+   * Returns the maximum button index on a given joystick port.
+   *
+   * @param stick The joystick port number
+   * @return The maximum button index on the indicated joystick
+   */
+  static int GetStickButtonsMaximumIndex(int stick);
+
+  /**
+   * Returns the mask of available buttons on a given joystick port.
+   *
+   * @param stick The joystick port number
+   * @return The mask of available buttons on the indicated joystick
+   */
+  static uint64_t GetStickButtonsAvailable(int stick);
+
+  /**
+   * Returns a boolean indicating if the controller is an xbox controller.
+   *
+   * @param stick The joystick port number
+   * @return A boolean that is true if the controller is an xbox controller.
+   */
+  static bool GetJoystickIsGamepad(int stick);
+
+  /**
+   * Returns the type of joystick at a given port.
+   *
+   * @param stick The joystick port number
+   * @return The HID type of joystick at the given port
+   */
+  static int GetJoystickType(int stick);
+
+  /**
+   * Returns the name of the joystick at the given port.
+   *
+   * @param stick The joystick port number
+   * @return The name of the joystick at the given port
+   */
+  static std::string GetJoystickName(int stick);
+
+  /**
+   * Returns if a joystick is connected to the Driver Station.
+   *
+   * This makes a best effort guess by looking at the reported number of axis,
+   * buttons, and POVs attached.
+   *
+   * @param stick The joystick port number
+   * @return true if a joystick is connected
+   */
+  static bool IsJoystickConnected(int stick);
+
+  /**
+   * Check if the DS has enabled the robot.
+   *
+   * @return True if the robot is enabled and the DS is connected
+   */
+  static bool IsEnabled();
+
+  /**
+   * Check if the robot is disabled.
+   *
+   * @return True if the robot is explicitly disabled or the DS is not connected
+   */
+  static bool IsDisabled();
+
+  /**
+   * Check if the robot is e-stopped.
+   *
+   * @return True if the robot is e-stopped
+   */
+  static bool IsEStopped();
+
+  /**
+   * Check if the DS is commanding autonomous mode.
+   *
+   * @return True if the robot is being commanded to be in autonomous mode
+   */
+  static bool IsAutonomous();
+
+  /**
+   * Check if the DS is commanding autonomous mode and if it has enabled the
+   * robot.
+   *
+   * @return True if the robot is being commanded to be in autonomous mode and
+   * enabled.
+   */
+  static bool IsAutonomousEnabled();
+
+  /**
+   * Check if the DS is commanding teleop mode.
+   *
+   * @return True if the robot is being commanded to be in teleop mode
+   */
+  static bool IsTeleop();
+
+  /**
+   * Check if the DS is commanding teleop mode and if it has enabled the robot.
+   *
+   * @return True if the robot is being commanded to be in teleop mode and
+   * enabled.
+   */
+  static bool IsTeleopEnabled();
+
+  /**
+   * Check if the DS is commanding test mode.
+   *
+   * @return True if the robot is being commanded to be in test mode
+   */
+  static bool IsTest();
+
+  /**
+   * Check if the DS is commanding Test mode and if it has enabled the robot.
+   *
+   * @return True if the robot is being commanded to be in Test mode and
+   * enabled.
+   */
+  static bool IsTestEnabled();
+
+  /**
+   * Check if the DS is attached.
+   *
+   * @return True if the DS is connected to the robot
+   */
+  static bool IsDSAttached();
+
+  /**
+   * Is the driver station attached to a Field Management System?
+   *
+   * @return True if the robot is competing on a field being controlled by a
+   *         Field Management System
+   */
+  static bool IsFMSAttached();
+
+  /**
+   * Returns the game specific message provided by the FMS.
+   *
+   * If the FMS is not connected, it is set from the game data setting on the
+   * driver station.
+   *
+   * @return A string containing the game specific message.
+   */
+  static std::string GetGameSpecificMessage();
+
+  /**
+   * Returns the name of the competition event provided by the FMS.
+   *
+   * @return A string containing the event name
+   */
+  static std::string GetEventName();
+
+  /**
+   * Returns the type of match being played provided by the FMS.
+   *
+   * @return The match type enum (kNone, kPractice, kQualification,
+   *         kElimination)
+   */
+  static MatchType GetMatchType();
+
+  /**
+   * Returns the match number provided by the FMS.
+   *
+   * @return The number of the match
+   */
+  static int GetMatchNumber();
+
+  /**
+   * Returns the number of times the current match has been replayed from the
+   * FMS.
+   *
+   * @return The number of replays
+   */
+  static int GetReplayNumber();
+
+  /**
+   * Get the current alliance from the FMS.
+   *
+   * If the FMS is not connected, it is set from the team alliance setting on
+   * the driver station.
+   *
+   * @return The alliance (red or blue) or an empty optional if the alliance is
+   * invalid
+   */
+  static std::optional<Alliance> GetAlliance();
+
+  /**
+   * Return the driver station location from the FMS.
+   *
+   * If the FMS is not connected, it is set from the team alliance setting on
+   * the driver station.
+   *
+   * This could return 1, 2, or 3.
+   *
+   * @return The location of the driver station (1-3, 0 for invalid)
+   */
+  static std::optional<int> GetLocation();
+
+  /**
+   * Return the approximate match time. The FMS does not send an official match
+   * time to the robots, but does send an approximate match time. The value will
+   * count down the time remaining in the current period (auto or teleop).
+   * Warning: This is not an official time (so it cannot be used to dispute ref
+   * calls or guarantee that a function will trigger before the match ends).
+   *
+   * <p>When connected to the real field, this number only changes in full
+   * integer increments, and always counts down.
+   *
+   * <p>When the DS is in practice mode, this number is a floating point number,
+   * and counts down.
+   *
+   * <p>When the DS is in teleop or autonomous mode, this number is a floating
+   * point number, and counts up.
+   *
+   * <p>Simulation matches DS behavior without an FMS connected.
+   *
+   * @return Time remaining in current match period (auto or teleop) in seconds
+   */
+  static units::second_t GetMatchTime();
+
+  /**
+   * Read the battery voltage.
+   *
+   * @return The battery voltage in Volts.
+   */
+  static double GetBatteryVoltage();
+
+  /**
+   * Copy data from the DS task for the user. If no new data exists, it will
+   * just be returned, otherwise the data will be copied from the DS polling
+   * loop.
+   */
+  static void RefreshData();
+
+  /**
+   * Registers the given handle for DS data refresh notifications.
+   *
+   * @param handle The event handle.
+   */
+  static void ProvideRefreshedDataEventHandle(WPI_EventHandle handle);
+
+  /**
+   * Unregisters the given handle from DS data refresh notifications.
+   *
+   * @param handle The event handle.
+   */
+  static void RemoveRefreshedDataEventHandle(WPI_EventHandle handle);
+
+  /**
+   * Allows the user to specify whether they want joystick connection warnings
+   * to be printed to the console. This setting is ignored when the FMS is
+   * connected -- warnings will always be on in that scenario.
+   *
+   * @param silence Whether warning messages should be silenced.
+   */
+  static void SilenceJoystickConnectionWarning(bool silence);
+
+  /**
+   * Returns whether joystick connection warnings are silenced. This will
+   * always return false when connected to the FMS.
+   *
+   * @return Whether joystick connection warnings are silenced.
+   */
+  static bool IsJoystickConnectionWarningSilenced();
+
+  /**
+   * Starts logging DriverStation data to data log. Repeated calls are ignored.
+   *
+   * @param log data log
+   * @param logJoysticks if true, log joystick data
+   */
+  static void StartDataLog(wpi::log::DataLog& log, bool logJoysticks = true);
+
+ private:
+  DriverStation() = default;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/driverstation/GenericHID.hpp

@@ -0,0 +1,370 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <string>
+
+#include "frc/DriverStation.h"
+
+namespace frc {
+
+class BooleanEvent;
+class EventLoop;
+
+/**
+ * Handle input from standard HID devices connected to the Driver Station.
+ *
+ * <p>This class handles standard input that comes from the Driver Station. Each
+ * time a value is requested the most recent value is returned. There is a
+ * single class instance for each device and the mapping of ports to hardware
+ * buttons depends on the code in the Driver Station.
+ */
+class GenericHID {
+ public:
+  /**
+   * Represents a rumble output on the Joystick.
+   */
+  enum RumbleType {
+    /// Left rumble motor.
+    kLeftRumble,
+    /// Right rumble motor.
+    kRightRumble,
+    /// Both left and right rumble motors.
+    kBothRumble
+  };
+
+  /**
+   * USB HID interface type.
+   */
+  enum HIDType {
+    /// Unknown.
+    kUnknown = -1,
+    /// XInputUnknown.
+    kXInputUnknown = 0,
+    /// XInputGamepad.
+    kXInputGamepad = 1,
+    /// XInputWheel.
+    kXInputWheel = 2,
+    /// XInputArcadeStick.
+    kXInputArcadeStick = 3,
+    /// XInputFlightStick.
+    kXInputFlightStick = 4,
+    /// XInputDancePad.
+    kXInputDancePad = 5,
+    /// XInputGuitar.
+    kXInputGuitar = 6,
+    /// XInputGuitar2.
+    kXInputGuitar2 = 7,
+    /// XInputDrumKit.
+    kXInputDrumKit = 8,
+    /// XInputGuitar3.
+    kXInputGuitar3 = 11,
+    /// XInputArcadePad.
+    kXInputArcadePad = 19,
+    /// HIDJoystick.
+    kHIDJoystick = 20,
+    /// HIDGamepad.
+    kHIDGamepad = 21,
+    /// HIDDriving.
+    kHIDDriving = 22,
+    /// HIDFlight.
+    kHIDFlight = 23,
+    /// HID1stPerson.
+    kHID1stPerson = 24
+  };
+
+  explicit GenericHID(int port);
+  virtual ~GenericHID() = default;
+
+  GenericHID(GenericHID&&) = default;
+  GenericHID& operator=(GenericHID&&) = default;
+
+  /**
+   * Get the button value (starting at button 1).
+   *
+   * The buttons are returned in a single 16 bit value with one bit representing
+   * the state of each button. The appropriate button is returned as a boolean
+   * value.
+   *
+   * This method returns true if the button is being held down at the time
+   * that this method is being called.
+   *
+   * @param button The button number to be read (starting at 1)
+   * @return The state of the button.
+   */
+  bool GetRawButton(int button) const;
+
+  /**
+   * Whether the button was pressed since the last check. %Button indexes begin
+   * at 1.
+   *
+   * This method returns true if the button went from not pressed to held down
+   * since the last time this method was called. This is useful if you only
+   * want to call a function once when you press the button.
+   *
+   * @param button The button index, beginning at 0.
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetRawButtonPressed(int button);
+
+  /**
+   * Whether the button was released since the last check. %Button indexes begin
+   * at 1.
+   *
+   * This method returns true if the button went from held down to not pressed
+   * since the last time this method was called. This is useful if you only
+   * want to call a function once when you release the button.
+   *
+   * @param button The button index, beginning at 0.
+   * @return Whether the button was released since the last check.
+   */
+  bool GetRawButtonReleased(int button);
+
+  /**
+   * Constructs an event instance around this button's digital signal.
+   *
+   * @param button the button index
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the button's digital signal attached
+   * to the given loop.
+   */
+  BooleanEvent Button(int button, EventLoop* loop) const;
+
+  /**
+   * Get the value of the axis.
+   *
+   * @param axis The axis to read, starting at 0.
+   * @return The value of the axis.
+   */
+  double GetRawAxis(int axis) const;
+
+  /**
+   * Get the angle of a POV on the HID.
+   *
+   * @param pov The index of the POV to read (starting at 0)
+   * @return the angle of the POV.
+   */
+  DriverStation::POVDirection GetPOV(int pov = 0) const;
+
+  /**
+   * Constructs a BooleanEvent instance based around this angle of a POV on the
+   * HID.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @param angle POV angle.
+   * @return a BooleanEvent instance based around this angle of a POV on the
+   * HID.
+   */
+  BooleanEvent POV(DriverStation::POVDirection angle, EventLoop* loop) const;
+
+  /**
+   * Constructs a BooleanEvent instance based around this angle of a POV on the
+   * HID.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @param pov   index of the POV to read (starting at 0). Defaults to 0.
+   * @param angle POV angle.
+   * @return a BooleanEvent instance based around this angle of a POV on the
+   * HID.
+   */
+  BooleanEvent POV(int pov, DriverStation::POVDirection angle,
+                   EventLoop* loop) const;
+
+  /**
+   * Constructs a BooleanEvent instance based around the up direction of
+   * the default (index 0) POV on the HID.
+   *
+   * @return a BooleanEvent instance based around the up direction of a POV on
+   * the HID.
+   */
+  BooleanEvent POVUp(EventLoop* loop) const;
+
+  /**
+   * Constructs a BooleanEvent instance based around the up right direction
+   * of the default (index 0) POV on the HID.
+   *
+   * @return a BooleanEvent instance based around the up right direction of a
+   * POV on the HID.
+   */
+  BooleanEvent POVUpRight(EventLoop* loop) const;
+
+  /**
+   * Constructs a BooleanEvent instance based around the right direction
+   * of the default (index 0) POV on the HID.
+   *
+   * @return a BooleanEvent instance based around the right direction of a POV
+   * on the HID.
+   */
+  BooleanEvent POVRight(EventLoop* loop) const;
+
+  /**
+   * Constructs a BooleanEvent instance based around the down right direction
+   * of the default (index 0) POV on the HID.
+   *
+   * @return a BooleanEvent instance based around the down right direction of a
+   * POV on the HID.
+   */
+  BooleanEvent POVDownRight(EventLoop* loop) const;
+
+  /**
+   * Constructs a BooleanEvent instance based around the down direction
+   * of the default (index 0) POV on the HID.
+   *
+   * @return a BooleanEvent instance based around the down direction of a POV
+   * on the HID.
+   */
+  BooleanEvent POVDown(EventLoop* loop) const;
+
+  /**
+   * Constructs a BooleanEvent instance based around the down left direction
+   * of the default (index 0) POV on the HID.
+   *
+   * @return a BooleanEvent instance based around the down left direction of a
+   * POV on the HID.
+   */
+  BooleanEvent POVDownLeft(EventLoop* loop) const;
+
+  /**
+   * Constructs a BooleanEvent instance based around the left direction
+   * of the default (index 0) POV on the HID.
+   *
+   * @return a BooleanEvent instance based around the left direction of a POV
+   * on the HID.
+   */
+  BooleanEvent POVLeft(EventLoop* loop) const;
+
+  /**
+   * Constructs a BooleanEvent instance based around the up left direction
+   * of the default (index 0) POV on the HID.
+   *
+   * @return a BooleanEvent instance based around the up left direction of a POV
+   * on the HID.
+   */
+  BooleanEvent POVUpLeft(EventLoop* loop) const;
+
+  /**
+   * Constructs a BooleanEvent instance based around the center (not pressed) of
+   * the default (index 0) POV on the HID.
+   *
+   * @return a BooleanEvent instance based around the center of a POV on the
+   * HID.
+   */
+  BooleanEvent POVCenter(EventLoop* loop) const;
+
+  /**
+   * Constructs an event instance that is true when the axis value is less than
+   * threshold
+   *
+   * @param axis The axis to read, starting at 0.
+   * @param threshold The value below which this trigger should return true.
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance that is true when the axis value is less than the
+   * provided threshold.
+   */
+  BooleanEvent AxisLessThan(int axis, double threshold, EventLoop* loop) const;
+
+  /**
+   * Constructs an event instance that is true when the axis value is greater
+   * than threshold
+   *
+   * @param axis The axis to read, starting at 0.
+   * @param threshold The value above which this trigger should return true.
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance that is true when the axis value is greater than
+   * the provided threshold.
+   */
+  BooleanEvent AxisGreaterThan(int axis, double threshold,
+                               EventLoop* loop) const;
+
+  int GetAxesMaximumIndex() const;
+
+  /**
+   * Get the number of axes for the HID.
+   *
+   * @return the number of axis for the current HID
+   */
+  int GetAxesAvailable() const;
+
+  int GetPOVsMaximumIndex() const;
+
+  /**
+   * Get the number of POVs for the HID.
+   *
+   * @return the number of POVs for the current HID
+   */
+  int GetPOVsAvailable() const;
+
+  int GetButtonsMaximumIndex() const;
+
+  /**
+   * Get the number of buttons for the HID.
+   *
+   * @return the number of buttons on the current HID
+   */
+  uint64_t GetButtonsAvailable() const;
+
+  /**
+   * Get if the HID is connected.
+   *
+   * @return true if the HID is connected
+   */
+  bool IsConnected() const;
+
+  /**
+   * Get the type of the HID.
+   *
+   * @return the type of the HID.
+   */
+  GenericHID::HIDType GetType() const;
+
+  /**
+   * Get the name of the HID.
+   *
+   * @return the name of the HID.
+   */
+  std::string GetName() const;
+
+  /**
+   * Get the port number of the HID.
+   *
+   * @return The port number of the HID.
+   */
+  int GetPort() const;
+
+  /**
+   * Set a single HID output value for the HID.
+   *
+   * @param outputNumber The index of the output to set (1-32)
+   * @param value        The value to set the output to
+   */
+  void SetOutput(int outputNumber, bool value);
+
+  /**
+   * Set all output values for the HID.
+   *
+   * @param value The 32 bit output value (1 bit for each output)
+   */
+  void SetOutputs(int value);
+
+  /**
+   * Set the rumble output for the HID.
+   *
+   * The DS currently supports 2 rumble values, left rumble and right rumble.
+   *
+   * @param type  Which rumble value to set
+   * @param value The normalized value (0 to 1) to set the rumble to
+   */
+  void SetRumble(RumbleType type, double value);
+
+ private:
+  int m_port;
+  int m_outputs = 0;
+  uint16_t m_leftRumble = 0;
+  uint16_t m_rightRumble = 0;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/driverstation/Joystick.hpp

@@ -0,0 +1,272 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+
+#include <units/angle.h>
+
+#include "frc/GenericHID.h"
+
+namespace frc {
+
+/**
+ * Handle input from standard Joysticks connected to the Driver Station.
+ *
+ * This class handles standard input that comes from the Driver Station. Each
+ * time a value is requested the most recent value is returned. There is a
+ * single class instance for each joystick and the mapping of ports to hardware
+ * buttons depends on the code in the Driver Station.
+ */
+class Joystick : public GenericHID {
+ public:
+  /// Default X axis channel.
+  static constexpr int kDefaultXChannel = 0;
+  /// Default Y axis channel.
+  static constexpr int kDefaultYChannel = 1;
+  /// Default Z axis channel.
+  static constexpr int kDefaultZChannel = 2;
+  /// Default twist axis channel.
+  static constexpr int kDefaultTwistChannel = 2;
+  /// Default throttle axis channel.
+  static constexpr int kDefaultThrottleChannel = 3;
+
+  /**
+   * Represents an analog axis on a joystick.
+   */
+  enum AxisType {
+    /// X axis.
+    kXAxis,
+    /// Y axis.
+    kYAxis,
+    /// Z axis.
+    kZAxis,
+    /// Twist axis.
+    kTwistAxis,
+    /// Throttle axis.
+    kThrottleAxis
+  };
+
+  /**
+   * Represents a digital button on a joystick.
+   */
+  enum ButtonType {
+    /// kTrigger.
+    kTriggerButton,
+    /// kTop.
+    kTopButton
+  };
+
+  /**
+   * Construct an instance of a joystick.
+   *
+   * The joystick index is the USB port on the Driver Station.
+   *
+   * @param port The port on the Driver Station that the joystick is plugged
+   *             into (0-5).
+   */
+  explicit Joystick(int port);
+
+  ~Joystick() override = default;
+
+  Joystick(Joystick&&) = default;
+  Joystick& operator=(Joystick&&) = default;
+
+  /**
+   * Set the channel associated with the X axis.
+   *
+   * @param channel The channel to set the axis to.
+   */
+  void SetXChannel(int channel);
+
+  /**
+   * Set the channel associated with the Y axis.
+   *
+   * @param channel The channel to set the axis to.
+   */
+  void SetYChannel(int channel);
+
+  /**
+   * Set the channel associated with the Z axis.
+   *
+   * @param channel The channel to set the axis to.
+   */
+  void SetZChannel(int channel);
+
+  /**
+   * Set the channel associated with the twist axis.
+   *
+   * @param channel The channel to set the axis to.
+   */
+  void SetTwistChannel(int channel);
+
+  /**
+   * Set the channel associated with the throttle axis.
+   *
+   * @param channel The channel to set the axis to.
+   */
+  void SetThrottleChannel(int channel);
+
+  /**
+   * Get the channel currently associated with the X axis.
+   *
+   * @return The channel for the axis.
+   */
+  int GetXChannel() const;
+
+  /**
+   * Get the channel currently associated with the Y axis.
+   *
+   * @return The channel for the axis.
+   */
+  int GetYChannel() const;
+
+  /**
+   * Get the channel currently associated with the Z axis.
+   *
+   * @return The channel for the axis.
+   */
+  int GetZChannel() const;
+
+  /**
+   * Get the channel currently associated with the twist axis.
+   *
+   * @return The channel for the axis.
+   */
+  int GetTwistChannel() const;
+
+  /**
+   * Get the channel currently associated with the throttle axis.
+   *
+   * @return The channel for the axis.
+   */
+  int GetThrottleChannel() const;
+
+  /**
+   * Get the X value of the current joystick.
+   *
+   * This depends on the mapping of the joystick connected to the current port.
+   * On most joysticks, positive is to the right.
+   */
+  double GetX() const;
+
+  /**
+   * Get the Y value of the current joystick.
+   *
+   * This depends on the mapping of the joystick connected to the current port.
+   * On most joysticks, positive is to the back.
+   */
+  double GetY() const;
+
+  /**
+   * Get the Z value of the current joystick.
+   *
+   * This depends on the mapping of the joystick connected to the current port.
+   */
+  double GetZ() const;
+
+  /**
+   * Get the twist value of the current joystick.
+   *
+   * This depends on the mapping of the joystick connected to the current port.
+   */
+  double GetTwist() const;
+
+  /**
+   * Get the throttle value of the current joystick.
+   *
+   * This depends on the mapping of the joystick connected to the current port.
+   */
+  double GetThrottle() const;
+
+  /**
+   * Read the state of the trigger on the joystick.
+   *
+   * Look up which button has been assigned to the trigger and read its state.
+   *
+   * @return The state of the trigger.
+   */
+  bool GetTrigger() const;
+
+  /**
+   * Whether the trigger was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetTriggerPressed();
+
+  /**
+   * Whether the trigger was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetTriggerReleased();
+
+  /**
+   * Constructs an event instance around the trigger button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the trigger button's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent Trigger(EventLoop* loop) const;
+
+  /**
+   * Read the state of the top button on the joystick.
+   *
+   * Look up which button has been assigned to the top and read its state.
+   *
+   * @return The state of the top button.
+   */
+  bool GetTop() const;
+
+  /**
+   * Whether the top button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetTopPressed();
+
+  /**
+   * Whether the top button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetTopReleased();
+
+  /**
+   * Constructs an event instance around the top button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the top button's digital signal
+   * attached to the given loop.
+   */
+  BooleanEvent Top(EventLoop* loop) const;
+
+  /**
+   * Get the magnitude of the vector formed by the joystick's
+   * current position relative to its origin.
+   *
+   * @return The magnitude of the direction vector
+   */
+  double GetMagnitude() const;
+
+  /**
+   * Get the direction of the vector formed by the joystick and its origin. 0 is
+   * forward and clockwise is positive. (Straight right is π/2 radians or 90
+   * degrees.)
+   *
+   * @return The direction of the vector.
+   */
+  units::radian_t GetDirection() const;
+
+ private:
+  enum Axis { kX, kY, kZ, kTwist, kThrottle, kNumAxes };
+  enum Button { kTrigger = 1, kTop = 2 };
+
+  std::array<int, Axis::kNumAxes> m_axes;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/event/BooleanEvent.hpp

@@ -0,0 +1,140 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/filter/Debouncer.h>
+
+#include <functional>
+#include <memory>
+
+#include <units/time.h>
+#include <wpi/FunctionExtras.h>
+
+#include "EventLoop.h"
+
+namespace frc {
+
+/**
+ * This class provides an easy way to link actions to active high logic signals.
+ * Each object represents a digital signal to which callback actions can be
+ * bound using {@link #IfHigh(std::function<void()>)}.
+ *
+ * <p>BooleanEvents can easily be composed for advanced functionality using
+ * {@link #operator&&}, {@link #operator||}, and {@link #operator!}.
+ *
+ * <p>To get a new BooleanEvent that triggers when this one changes see {@link
+ * #Falling()} and {@link #Rising()}.
+ */
+class BooleanEvent {
+ public:
+  /**
+   * Creates a new event that is active when the condition is true.
+   *
+   * @param loop the loop that polls this event
+   * @param signal the digital signal represented by this object.
+   */
+  BooleanEvent(EventLoop* loop, std::function<bool()> signal);
+
+  /**
+   * Returns the state of this signal (high or low) as of the last loop poll.
+   *
+   * @return true for the high state, false for the low state. If the event was
+   * never polled, it returns the state at event construction.
+   */
+  bool GetAsBoolean() const;
+
+  operator std::function<bool()>();  // NOLINT
+
+  /**
+   * Bind an action to this event.
+   *
+   * @param action the action to run if this event is active.
+   */
+  void IfHigh(std::function<void()> action);
+
+  /**
+   * A method to "downcast" a BooleanEvent instance to a subclass (for example,
+   * to a command-based version of this class).
+   *
+   * @param ctor a method reference to the constructor of the subclass that
+   * accepts the loop as the first parameter and the condition/signal as the
+   * second.
+   * @return an instance of the subclass.
+   */
+  template <class T>
+  T CastTo(std::function<T(EventLoop*, std::function<bool()>)> ctor =
+               [](EventLoop* loop, std::function<bool()> condition) {
+                 return T(loop, condition);
+               }) {
+    return ctor(m_loop, [state = m_state] { return *state; });
+  }
+
+  /**
+   * Creates a new event that is active when this event is inactive.
+   *
+   * @return the negated event
+   */
+  BooleanEvent operator!();
+
+  /**
+   * Composes this event with another event, returning a new event that is
+   * active when both events are active.
+   *
+   * <p>The events must use the same event loop. If the events use different
+   * event loops, the composed signal won't update until both loops are polled.
+   *
+   * @param rhs the event to compose with
+   * @return the event that is active when both events are active
+   */
+  BooleanEvent operator&&(std::function<bool()> rhs);
+
+  /**
+   * Composes this event with another event, returning a new event that is
+   * active when either event is active.
+   *
+   * <p>The events must use the same event loop. If the events use different
+   * event loops, the composed signal won't update until both loops are polled.
+   *
+   * @param rhs the event to compose with
+   * @return the event that is active when either event is active
+   */
+  BooleanEvent operator||(std::function<bool()> rhs);
+
+  /**
+   * Creates a new event that triggers when this one changes from false to true.
+   *
+   * @return the new event.
+   */
+  BooleanEvent Rising();
+
+  /**
+   * Creates a new event that triggers when this one changes from true to false.
+   *
+   * @return the event.
+   */
+  BooleanEvent Falling();
+
+  /**
+   * Creates a new debounced event from this event - it will become active when
+   * this event has been active for longer than the specified period.
+   *
+   * @param debounceTime The debounce period.
+   * @param type The debounce type.
+   * @return The debounced event.
+   */
+  BooleanEvent Debounce(units::second_t debounceTime,
+                        frc::Debouncer::DebounceType type =
+                            frc::Debouncer::DebounceType::kRising);
+
+ private:
+  /// Poller loop.
+  EventLoop* m_loop;
+
+  std::function<bool()> m_signal;
+
+  /// The state of the condition in the current loop poll.
+  std::shared_ptr<bool> m_state;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/event/EventLoop.hpp

@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+#include <wpi/FunctionExtras.h>
+
+namespace frc {
+/** A declarative way to bind a set of actions to a loop and execute them when
+ * the loop is polled. */
+class EventLoop {
+ public:
+  EventLoop();
+
+  EventLoop(const EventLoop&) = delete;
+  EventLoop& operator=(const EventLoop&) = delete;
+
+  /**
+   * Bind a new action to run when the loop is polled.
+   *
+   * @param action the action to run.
+   */
+  void Bind(wpi::unique_function<void()> action);
+
+  /**
+   * Poll all bindings.
+   */
+  void Poll();
+
+  /**
+   * Clear all bindings.
+   */
+  void Clear();
+
+ private:
+  std::vector<wpi::unique_function<void()>> m_bindings;
+  bool m_running{false};
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/event/NetworkBooleanEvent.hpp

@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string_view>
+
+#include "BooleanEvent.h"
+
+namespace nt {
+class BooleanSubscriber;
+class BooleanTopic;
+class NetworkTable;
+class NetworkTableInstance;
+}  // namespace nt
+
+namespace frc {
+/**
+ * A Button that uses a NetworkTable boolean field.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class NetworkBooleanEvent : public BooleanEvent {
+ public:
+  /**
+   * Creates a new event with the given boolean topic determining whether it is
+   * active.
+   *
+   * @param loop the loop that polls this event
+   * @param topic The boolean topic that contains the value
+   */
+  NetworkBooleanEvent(EventLoop* loop, nt::BooleanTopic topic);
+
+  /**
+   * Creates a new event with the given boolean subscriber determining whether
+   * it is active.
+   *
+   * @param loop the loop that polls this event
+   * @param sub The boolean subscriber that provides the value
+   */
+  NetworkBooleanEvent(EventLoop* loop, nt::BooleanSubscriber sub);
+
+  /**
+   * Creates a new event with the given boolean topic determining whether it is
+   * active.
+   *
+   * @param loop the loop that polls this event
+   * @param table The NetworkTable that contains the topic
+   * @param topicName The topic name within the table that contains the value
+   */
+  NetworkBooleanEvent(EventLoop* loop, std::shared_ptr<nt::NetworkTable> table,
+                      std::string_view topicName);
+
+  /**
+   * Creates a new event with the given boolean topic determining whether it is
+   * active.
+   *
+   * @param loop the loop that polls this event
+   * @param tableName The NetworkTable name that contains the topic
+   * @param topicName The topic name within the table that contains the value
+   */
+  NetworkBooleanEvent(EventLoop* loop, std::string_view tableName,
+                      std::string_view topicName);
+
+  /**
+   * Creates a new event with the given boolean topic determining whether it is
+   * active.
+   *
+   * @param loop the loop that polls this event
+   * @param inst The NetworkTable instance to use
+   * @param tableName The NetworkTable that contains the topic
+   * @param topicName The topic name within the table that contains the value
+   */
+  NetworkBooleanEvent(EventLoop* loop, nt::NetworkTableInstance inst,
+                      std::string_view tableName, std::string_view topicName);
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/accelerometer/ADXL345_I2C.hpp

@@ -0,0 +1,160 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <networktables/NTSendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/I2C.h"
+
+namespace frc {
+
+/**
+ * ADXL345 Accelerometer on I2C.
+ *
+ * This class allows access to a Analog Devices ADXL345 3-axis accelerometer on
+ * an I2C bus. This class assumes the default (not alternate) sensor address of
+ * 0x1D (7-bit address).
+ */
+class ADXL345_I2C : public nt::NTSendable,
+                    public wpi::SendableHelper<ADXL345_I2C> {
+ public:
+  /**
+   * Accelerometer range.
+   */
+  enum Range {
+    /// 2 Gs max.
+    kRange_2G = 0,
+    /// 4 Gs max.
+    kRange_4G = 1,
+    /// 8 Gs max.
+    kRange_8G = 2,
+    /// 16 Gs max.
+    kRange_16G = 3
+  };
+
+  /**
+   * Accelerometer axes.
+   */
+  enum Axes {
+    /// X axis.
+    kAxis_X = 0x00,
+    /// Y axis.
+    kAxis_Y = 0x02,
+    /// Z axis.
+    kAxis_Z = 0x04
+  };
+
+  /**
+   * Container type for accelerations from all axes.
+   */
+  struct AllAxes {
+    /// Acceleration along the X axis in g-forces.
+    double XAxis = 0.0;
+    /// Acceleration along the Y axis in g-forces.
+    double YAxis = 0.0;
+    /// Acceleration along the Z axis in g-forces.
+    double ZAxis = 0.0;
+  };
+
+  /// Default I2C device address.
+  static constexpr int kAddress = 0x1D;
+
+  /**
+   * Constructs the ADXL345 Accelerometer over I2C.
+   *
+   * @param port          The I2C port the accelerometer is attached to
+   * @param range         The range (+ or -) that the accelerometer will measure
+   * @param deviceAddress The I2C address of the accelerometer (0x1D or 0x53)
+   */
+  explicit ADXL345_I2C(I2C::Port port, Range range = kRange_2G,
+                       int deviceAddress = kAddress);
+  ~ADXL345_I2C() override = default;
+
+  ADXL345_I2C(ADXL345_I2C&&) = default;
+  ADXL345_I2C& operator=(ADXL345_I2C&&) = default;
+
+  I2C::Port GetI2CPort() const;
+  int GetI2CDeviceAddress() const;
+
+  /**
+   * Set the measuring range of the accelerometer.
+   *
+   * @param range The maximum acceleration, positive or negative, that the
+   *     accelerometer will measure.
+   */
+  void SetRange(Range range);
+
+  /**
+   * Returns the acceleration along the X axis in g-forces.
+   *
+   * @return The acceleration along the X axis in g-forces.
+   */
+  double GetX();
+
+  /**
+   * Returns the acceleration along the Y axis in g-forces.
+   *
+   * @return The acceleration along the Y axis in g-forces.
+   */
+  double GetY();
+
+  /**
+   * Returns the acceleration along the Z axis in g-forces.
+   *
+   * @return The acceleration along the Z axis in g-forces.
+   */
+  double GetZ();
+
+  /**
+   * Get the acceleration of one axis in Gs.
+   *
+   * @param axis The axis to read from.
+   * @return Acceleration of the ADXL345 in Gs.
+   */
+  virtual double GetAcceleration(Axes axis);
+
+  /**
+   * Get the acceleration of all axes in Gs.
+   *
+   * @return An object containing the acceleration measured on each axis of the
+   *         ADXL345 in Gs.
+   */
+  virtual AllAxes GetAccelerations();
+
+  void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+  I2C m_i2c;
+
+  hal::SimDevice m_simDevice;
+  hal::SimEnum m_simRange;
+  hal::SimDouble m_simX;
+  hal::SimDouble m_simY;
+  hal::SimDouble m_simZ;
+
+  static constexpr int kPowerCtlRegister = 0x2D;
+  static constexpr int kDataFormatRegister = 0x31;
+  static constexpr int kDataRegister = 0x32;
+  static constexpr double kGsPerLSB = 0.00390625;
+
+  enum PowerCtlFields {
+    kPowerCtl_Link = 0x20,
+    kPowerCtl_AutoSleep = 0x10,
+    kPowerCtl_Measure = 0x08,
+    kPowerCtl_Sleep = 0x04
+  };
+
+  enum DataFormatFields {
+    kDataFormat_SelfTest = 0x80,
+    kDataFormat_SPI = 0x40,
+    kDataFormat_IntInvert = 0x20,
+    kDataFormat_FullRes = 0x08,
+    kDataFormat_Justify = 0x04
+  };
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/accelerometer/AnalogAccelerometer.hpp

@@ -0,0 +1,106 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/AnalogInput.h"
+
+namespace frc {
+
+/**
+ * Handle operation of an analog accelerometer.
+ *
+ * The accelerometer reads acceleration directly through the sensor. Many
+ * sensors have multiple axis and can be treated as multiple devices. Each is
+ * calibrated by finding the center value over a period of time.
+ */
+class AnalogAccelerometer : public wpi::Sendable,
+                            public wpi::SendableHelper<AnalogAccelerometer> {
+ public:
+  /**
+   * Create a new instance of an accelerometer.
+   *
+   * The constructor allocates desired analog input.
+   *
+   * @param channel The channel number for the analog input the accelerometer is
+   *                connected to
+   */
+  explicit AnalogAccelerometer(int channel);
+
+  /**
+   * Create a new instance of Accelerometer from an existing AnalogInput.
+   *
+   * Make a new instance of accelerometer given an AnalogInput. This is
+   * particularly useful if the port is going to be read as an analog channel as
+   * well as through the Accelerometer class.
+   *
+   * @param channel The existing AnalogInput object for the analog input the
+   *                accelerometer is connected to
+   */
+  explicit AnalogAccelerometer(AnalogInput* channel);
+
+  /**
+   * Create a new instance of Accelerometer from an existing AnalogInput.
+   *
+   * Make a new instance of accelerometer given an AnalogInput. This is
+   * particularly useful if the port is going to be read as an analog channel as
+   * well as through the Accelerometer class.
+   *
+   * @param channel The existing AnalogInput object for the analog input the
+   *                accelerometer is connected to
+   */
+  explicit AnalogAccelerometer(std::shared_ptr<AnalogInput> channel);
+
+  ~AnalogAccelerometer() override = default;
+
+  AnalogAccelerometer(AnalogAccelerometer&&) = default;
+  AnalogAccelerometer& operator=(AnalogAccelerometer&&) = default;
+
+  /**
+   * Return the acceleration in Gs.
+   *
+   * The acceleration is returned units of Gs.
+   *
+   * @return The current acceleration of the sensor in Gs.
+   */
+  double GetAcceleration() const;
+
+  /**
+   * Set the accelerometer sensitivity.
+   *
+   * This sets the sensitivity of the accelerometer used for calculating the
+   * acceleration. The sensitivity varies by accelerometer model.
+   *
+   * @param sensitivity The sensitivity of accelerometer in Volts per G.
+   */
+  void SetSensitivity(double sensitivity);
+
+  /**
+   * Set the voltage that corresponds to 0 G.
+   *
+   * The zero G voltage varies by accelerometer model.
+   *
+   * @param zero The zero G voltage.
+   */
+  void SetZero(double zero);
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  /**
+   * Common function for initializing the accelerometer.
+   */
+  void InitAccelerometer();
+
+  std::shared_ptr<AnalogInput> m_analogInput;
+  double m_voltsPerG = 1.0;
+  double m_zeroGVoltage = 2.5;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/bus/CAN.hpp

@@ -0,0 +1,159 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <hal/CANAPI.h>
+
+namespace frc {
+
+/**
+ * High level class for interfacing with CAN devices conforming to
+ * the standard CAN spec.
+ *
+ * No packets that can be sent gets blocked by the RoboRIO, so all methods
+ * work identically in all robot modes.
+ *
+ * All methods are thread save, however the buffer objects passed in
+ * by the user need to not be modified for the duration of their calls.
+ */
+class CAN {
+ public:
+  /**
+   * Create a new CAN communication interface with the specific device ID.
+   * This uses the team manufacturer and device types.
+   * The device ID is 6 bits (0-63)
+   *
+   * @param busId    The bus id
+   * @param deviceId The device id
+   */
+  CAN(int busId, int deviceId);
+
+  /**
+   * Create a new CAN communication interface with a specific device ID,
+   * manufacturer and device type. The device ID is 6 bits, the
+   * manufacturer is 8 bits, and the device type is 5 bits.
+   *
+   * @param busId              The bus id
+   * @param deviceId           The device ID
+   * @param deviceManufacturer The device manufacturer
+   * @param deviceType         The device type
+   */
+  CAN(int busId, int deviceId, int deviceManufacturer, int deviceType);
+
+  CAN(CAN&&) = default;
+  CAN& operator=(CAN&&) = default;
+
+  /**
+   * Write a packet to the CAN device with a specific ID. This ID is 10 bits.
+   *
+   * @param apiId The API ID to write.
+   * @param message the CAN message.
+   */
+  void WritePacket(int apiId, const HAL_CANMessage& message);
+
+  /**
+   * Write a repeating packet to the CAN device with a specific ID. This ID is
+   * 10 bits. The RoboRIO will automatically repeat the packet at the specified
+   * interval
+   *
+   * @param apiId The API ID to write.
+   * @param message the CAN message.
+   * @param repeatMs The period to repeat the packet at.
+   */
+  void WritePacketRepeating(int apiId, const HAL_CANMessage& message,
+                            int repeatMs);
+
+  /**
+   * Write an RTR frame to the CAN device with a specific ID. This ID is 10
+   * bits. The length by spec must match what is returned by the responding
+   * device
+   *
+   * @param apiId The API ID to write.
+   * @param message the CAN message.
+   */
+  void WriteRTRFrame(int apiId, const HAL_CANMessage& message);
+
+  /**
+   * Write a packet to the CAN device with a specific ID. This ID is 10 bits.
+   *
+   * @param apiId The API ID to write.
+   * @param message the CAN message.
+   */
+  int WritePacketNoError(int apiId, const HAL_CANMessage& message);
+
+  /**
+   * Write a repeating packet to the CAN device with a specific ID. This ID is
+   * 10 bits. The RoboRIO will automatically repeat the packet at the specified
+   * interval
+   *
+   * @param apiId The API ID to write.
+   * @param message the CAN message.
+   * @param repeatMs The period to repeat the packet at.
+   */
+  int WritePacketRepeatingNoError(int apiId, const HAL_CANMessage& message,
+                                  int repeatMs);
+
+  /**
+   * Write an RTR frame to the CAN device with a specific ID. This ID is 10
+   * bits. The length by spec must match what is returned by the responding
+   * device
+   *
+   * @param apiId The API ID to write.
+   * @param message the CAN message.
+   */
+  int WriteRTRFrameNoError(int apiId, const HAL_CANMessage& message);
+
+  /**
+   * Stop a repeating packet with a specific ID. This ID is 10 bits.
+   *
+   * @param apiId The API ID to stop repeating
+   */
+  void StopPacketRepeating(int apiId);
+
+  /**
+   * Read a new CAN packet. This will only return properly once per packet
+   * received. Multiple calls without receiving another packet will return
+   * false.
+   *
+   * @param apiId The API ID to read.
+   * @param data Storage for the received data.
+   * @return True if the data is valid, otherwise false.
+   */
+  bool ReadPacketNew(int apiId, HAL_CANReceiveMessage* data);
+
+  /**
+   * Read a CAN packet. The will continuously return the last packet received,
+   * without accounting for packet age.
+   *
+   * @param apiId The API ID to read.
+   * @param data Storage for the received data.
+   * @return True if the data is valid, otherwise false.
+   */
+  bool ReadPacketLatest(int apiId, HAL_CANReceiveMessage* data);
+
+  /**
+   * Read a CAN packet. The will return the last packet received until the
+   * packet is older then the requested timeout. Then it will return false.
+   *
+   * @param apiId The API ID to read.
+   * @param timeoutMs The timeout time for the packet
+   * @param data Storage for the received data.
+   * @return True if the data is valid, otherwise false.
+   */
+  bool ReadPacketTimeout(int apiId, int timeoutMs, HAL_CANReceiveMessage* data);
+
+  /// Team manufacturer.
+  static constexpr HAL_CANManufacturer kTeamManufacturer = HAL_CAN_Man_kTeamUse;
+
+  /// Team device type.
+  static constexpr HAL_CANDeviceType kTeamDeviceType =
+      HAL_CAN_Dev_kMiscellaneous;
+
+ private:
+  hal::Handle<HAL_CANHandle, HAL_CleanCAN> m_handle;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/bus/I2C.hpp

@@ -0,0 +1,160 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <hal/I2C.h>
+#include <hal/I2CTypes.h>
+
+namespace frc {
+
+/**
+ * I2C bus interface class.
+ *
+ * This class is intended to be used by sensor (and other I2C device) drivers.
+ * It probably should not be used directly.
+ */
+class I2C {
+ public:
+  /**
+   * I2C connection ports.
+   */
+  enum Port {
+    /// I2C Port 0.
+    kPort0 = 0,
+    /// I2C Port 1.
+    kPort1
+  };
+
+  /**
+   * Constructor.
+   *
+   * @param port          The I2C port to which the device is connected.
+   * @param deviceAddress The address of the device on the I2C bus.
+   */
+  I2C(Port port, int deviceAddress);
+
+  I2C(I2C&&) = default;
+  I2C& operator=(I2C&&) = default;
+
+  /**
+   * Returns I2C port.
+   *
+   * @return I2C port.
+   */
+  Port GetPort() const;
+
+  /**
+   * Returns I2C device address.
+   *
+   * @return I2C device address.
+   */
+  int GetDeviceAddress() const;
+
+  /**
+   * Generic transaction.
+   *
+   * This is a lower-level interface to the I2C hardware giving you more control
+   * over each transaction. If you intend to write multiple bytes in the same
+   * transaction and do not plan to receive anything back, use writeBulk()
+   * instead. Calling this with a receiveSize of 0 will result in an error.
+   *
+   * @param dataToSend   Buffer of data to send as part of the transaction.
+   * @param sendSize     Number of bytes to send as part of the transaction.
+   * @param dataReceived Buffer to read data into.
+   * @param receiveSize  Number of bytes to read from the device.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  bool Transaction(uint8_t* dataToSend, int sendSize, uint8_t* dataReceived,
+                   int receiveSize);
+
+  /**
+   * Attempt to address a device on the I2C bus.
+   *
+   * This allows you to figure out if there is a device on the I2C bus that
+   * responds to the address specified in the constructor.
+   *
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  bool AddressOnly();
+
+  /**
+   * Execute a write transaction with the device.
+   *
+   * Write a single byte to a register on a device and wait until the
+   *   transaction is complete.
+   *
+   * @param registerAddress The address of the register on the device to be
+   *                        written.
+   * @param data            The byte to write to the register on the device.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  bool Write(int registerAddress, uint8_t data);
+
+  /**
+   * Execute a bulk write transaction with the device.
+   *
+   * Write multiple bytes to a device and wait until the
+   *   transaction is complete.
+   *
+   * @param data  The data to write to the register on the device.
+   * @param count The number of bytes to be written.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  bool WriteBulk(uint8_t* data, int count);
+
+  /**
+   * Execute a read transaction with the device.
+   *
+   * Read bytes from a device.
+   * Most I2C devices will auto-increment the register pointer internally
+   * allowing you to read consecutive registers on a device in a single
+   * transaction.
+   *
+   * @param registerAddress The register to read first in the transaction.
+   * @param count           The number of bytes to read in the transaction.
+   * @param data            A pointer to the array of bytes to store the data
+   *                        read from the device.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  bool Read(int registerAddress, int count, uint8_t* data);
+
+  /**
+   * Execute a read only transaction with the device.
+   *
+   * Read bytes from a device. This method does not write any data to prompt the
+   * device.
+   *
+   * @param buffer A pointer to the array of bytes to store the data read from
+   *               the device.
+   * @param count  The number of bytes to read in the transaction.
+   * @return Transfer Aborted... false for success, true for aborted.
+   */
+  bool ReadOnly(int count, uint8_t* buffer);
+
+  /**
+   * Verify that a device's registers contain expected values.
+   *
+   * Most devices will have a set of registers that contain a known value that
+   * can be used to identify them.  This allows an I2C device driver to easily
+   * verify that the device contains the expected value.
+   *
+   * @pre The device must support and be configured to use register
+   * auto-increment.
+   *
+   * @param registerAddress The base register to start reading from the device.
+   * @param count           The size of the field to be verified.
+   * @param expected        A buffer containing the values expected from the
+   *                        device.
+   */
+  bool VerifySensor(int registerAddress, int count, const uint8_t* expected);
+
+ private:
+  hal::Handle<HAL_I2CPort, HAL_CloseI2C, HAL_I2C_kInvalid> m_port;
+  int m_deviceAddress;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/bus/SerialPort.hpp

@@ -0,0 +1,260 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <hal/SerialPort.h>
+#include <hal/Types.h>
+#include <units/time.h>
+
+namespace frc {
+
+/**
+ * Driver for the RS-232 serial port on the roboRIO.
+ *
+ * The current implementation uses the VISA formatted I/O mode.  This means that
+ * all traffic goes through the formatted buffers.  This allows the intermingled
+ * use of Printf(), Scanf(), and the raw buffer accessors Read() and Write().
+ *
+ * More information can be found in the NI-VISA User Manual here:
+ *   http://www.ni.com/pdf/manuals/370423a.pdf
+ * and the NI-VISA Programmer's Reference Manual here:
+ *   http://www.ni.com/pdf/manuals/370132c.pdf
+ */
+class SerialPort {
+ public:
+  /**
+   * Serial port.
+   */
+  enum Port {
+    /// Onboard serial port on the roboRIO.
+    kOnboard = 0,
+    /// MXP (roboRIO MXP) serial port.
+    kMXP = 1,
+    /// USB serial port (same as kUSB1).
+    kUSB = 2,
+    /// USB serial port 1.
+    kUSB1 = 2,
+    /// USB serial port 2.
+    kUSB2 = 3
+  };
+
+  /**
+   * Represents the parity to use for serial communications.
+   */
+  enum Parity {
+    /// No parity.
+    kParity_None = 0,
+    /// Odd parity.
+    kParity_Odd = 1,
+    /// Even parity.
+    kParity_Even = 2,
+    /// Parity bit always on.
+    kParity_Mark = 3,
+    /// Parity bit always off.
+    kParity_Space = 4
+  };
+
+  /**
+   * Represents the number of stop bits to use for Serial Communication.
+   */
+  enum StopBits {
+    /// One stop bit.
+    kStopBits_One = 10,
+    /// One and a half stop bits.
+    kStopBits_OnePointFive = 15,
+    /// Two stop bits.
+    kStopBits_Two = 20
+  };
+
+  /**
+   * Represents what type of flow control to use for serial communication.
+   */
+  enum FlowControl {
+    /// No flow control.
+    kFlowControl_None = 0,
+    /// XON/XOFF flow control.
+    kFlowControl_XonXoff = 1,
+    /// RTS/CTS flow control.
+    kFlowControl_RtsCts = 2,
+    /// DTS/DSR flow control.
+    kFlowControl_DtrDsr = 4
+  };
+
+  /**
+   * Represents which type of buffer mode to use when writing to a serial port.
+   */
+  enum WriteBufferMode {
+    /// Flush the buffer on each access.
+    kFlushOnAccess = 1,
+    /// Flush the buffer when it is full.
+    kFlushWhenFull = 2
+  };
+
+  /**
+   * Create an instance of a Serial Port class.
+   *
+   * @param baudRate The baud rate to configure the serial port.
+   * @param port     The physical port to use
+   * @param dataBits The number of data bits per transfer.  Valid values are
+   *                 between 5 and 8 bits.
+   * @param parity   Select the type of parity checking to use.
+   * @param stopBits The number of stop bits to use as defined by the enum
+   *                 StopBits.
+   */
+  explicit SerialPort(int baudRate, Port port = kOnboard, int dataBits = 8,
+                      Parity parity = kParity_None,
+                      StopBits stopBits = kStopBits_One);
+
+  /**
+   * Create an instance of a Serial Port class.
+   *
+   * Prefer to use the constructor that doesn't take a port name, but in some
+   * cases the automatic detection might not work correctly.
+   *
+   * @param baudRate The baud rate to configure the serial port.
+   * @param port     The physical port to use
+   * @param portName The direct port name to use
+   * @param dataBits The number of data bits per transfer.  Valid values are
+   *                 between 5 and 8 bits.
+   * @param parity   Select the type of parity checking to use.
+   * @param stopBits The number of stop bits to use as defined by the enum
+   *                 StopBits.
+   */
+  SerialPort(int baudRate, std::string_view portName, Port port = kOnboard,
+             int dataBits = 8, Parity parity = kParity_None,
+             StopBits stopBits = kStopBits_One);
+
+  SerialPort(SerialPort&& rhs) = default;
+  SerialPort& operator=(SerialPort&& rhs) = default;
+
+  /**
+   * Set the type of flow control to enable on this port.
+   *
+   * By default, flow control is disabled.
+   */
+  void SetFlowControl(FlowControl flowControl);
+
+  /**
+   * Enable termination and specify the termination character.
+   *
+   * Termination is currently only implemented for receive.
+   * When the the terminator is received, the Read() or Scanf() will return
+   * fewer bytes than requested, stopping after the terminator.
+   *
+   * @param terminator The character to use for termination.
+   */
+  void EnableTermination(char terminator = '\n');
+
+  /**
+   * Disable termination behavior.
+   */
+  void DisableTermination();
+
+  /**
+   * Get the number of bytes currently available to read from the serial port.
+   *
+   * @return The number of bytes available to read
+   */
+  int GetBytesReceived();
+
+  /**
+   * Read raw bytes out of the buffer.
+   *
+   * @param buffer Pointer to the buffer to store the bytes in.
+   * @param count  The maximum number of bytes to read.
+   * @return The number of bytes actually read into the buffer.
+   */
+  int Read(char* buffer, int count);
+
+  /**
+   * Write raw bytes to the buffer.
+   *
+   * @param buffer Pointer to the buffer to read the bytes from.
+   * @param count  The maximum number of bytes to write.
+   * @return The number of bytes actually written into the port.
+   */
+  int Write(const char* buffer, int count);
+
+  /**
+   * Write raw bytes to the buffer.
+   *
+   * Use Write({data, len}) to get a buffer that is shorter than the length of
+   * the string.
+   *
+   * @param buffer the buffer to read the bytes from.
+   * @return The number of bytes actually written into the port.
+   */
+  int Write(std::string_view buffer);
+
+  /**
+   * Configure the timeout of the serial port.
+   *
+   * This defines the timeout for transactions with the hardware.
+   * It will affect reads and very large writes.
+   *
+   * @param timeout The time to wait for I/O.
+   */
+  void SetTimeout(units::second_t timeout);
+
+  /**
+   * Specify the size of the input buffer.
+   *
+   * Specify the amount of data that can be stored before data
+   * from the device is returned to Read or Scanf.  If you want
+   * data that is received to be returned immediately, set this to 1.
+   *
+   * It the buffer is not filled before the read timeout expires, all
+   * data that has been received so far will be returned.
+   *
+   * @param size The read buffer size.
+   */
+  void SetReadBufferSize(int size);
+
+  /**
+   * Specify the size of the output buffer.
+   *
+   * Specify the amount of data that can be stored before being
+   * transmitted to the device.
+   *
+   * @param size The write buffer size.
+   */
+  void SetWriteBufferSize(int size);
+
+  /**
+   * Specify the flushing behavior of the output buffer.
+   *
+   * When set to kFlushOnAccess, data is synchronously written to the serial
+   * port after each call to either Printf() or Write().
+   *
+   * When set to kFlushWhenFull, data will only be written to the serial port
+   * when the buffer is full or when Flush() is called.
+   *
+   * @param mode The write buffer mode.
+   */
+  void SetWriteBufferMode(WriteBufferMode mode);
+
+  /**
+   * Force the output buffer to be written to the port.
+   *
+   * This is used when SetWriteBufferMode() is set to kFlushWhenFull to force a
+   * flush before the buffer is full.
+   */
+  void Flush();
+
+  /**
+   * Reset the serial port driver to a known state.
+   *
+   * Empty the transmit and receive buffers in the device and formatted I/O.
+   */
+  void Reset();
+
+ private:
+  hal::Handle<HAL_SerialPortHandle, HAL_CloseSerial> m_portHandle;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/discrete/AnalogInput.hpp

@@ -0,0 +1,204 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <hal/AnalogInput.h>
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+
+/**
+ * Analog input class.
+ *
+ * Connected to each analog channel is an averaging and oversampling engine.
+ * This engine accumulates the specified ( by SetAverageBits() and
+ * SetOversampleBits() ) number of samples before returning a new value. This is
+ * not a sliding window average. The only difference between the oversampled
+ * samples and the averaged samples is that the oversampled samples are simply
+ * accumulated effectively increasing the resolution, while the averaged samples
+ * are divided by the number of samples to retain the resolution, but get more
+ * stable values.
+ */
+class AnalogInput : public wpi::Sendable,
+                    public wpi::SendableHelper<AnalogInput> {
+ public:
+  /**
+   * Construct an analog input.
+   *
+   * @param channel The channel number on the roboRIO to represent. 0-3 are
+   *                on-board 4-7 are on the MXP port.
+   */
+  explicit AnalogInput(int channel);
+
+  AnalogInput(AnalogInput&&) = default;
+  AnalogInput& operator=(AnalogInput&&) = default;
+
+  ~AnalogInput() override = default;
+
+  /**
+   * Get a sample straight from this channel.
+   *
+   * The sample is a 12-bit value representing the 0V to 3.3V range of the A/D
+   * converter in the module.  The units are in A/D converter codes.  Use
+   * GetVoltage() to get the analog value in calibrated units.
+   *
+   * @return A sample straight from this channel.
+   */
+  int GetValue() const;
+
+  /**
+   * Get a sample from the output of the oversample and average engine for this
+   * channel.
+   *
+   * The sample is 12-bit + the bits configured in SetOversampleBits().
+   * The value configured in SetAverageBits() will cause this value to be
+   * averaged 2**bits number of samples.
+   *
+   * This is not a sliding window. The sample will not change until
+   * 2**(OversampleBits + AverageBits) samples have been acquired from the
+   * module on this channel.
+   *
+   * Use GetAverageVoltage() to get the analog value in calibrated units.
+   *
+   * @return A sample from the oversample and average engine for this channel.
+   */
+  int GetAverageValue() const;
+
+  /**
+   * Get a scaled sample straight from this channel.
+   *
+   * The value is scaled to units of Volts using the calibrated scaling data
+   * from GetLSBWeight() and GetOffset().
+   *
+   * @return A scaled sample straight from this channel.
+   */
+  double GetVoltage() const;
+
+  /**
+   * Get a scaled sample from the output of the oversample and average engine
+   * for this channel.
+   *
+   * The value is scaled to units of Volts using the calibrated scaling data
+   * from GetLSBWeight() and GetOffset().
+   *
+   * Using oversampling will cause this value to be higher resolution, but it
+   * will update more slowly.
+   *
+   * Using averaging will cause this value to be more stable, but it will update
+   * more slowly.
+   *
+   * @return A scaled sample from the output of the oversample and average
+   * engine for this channel.
+   */
+  double GetAverageVoltage() const;
+
+  /**
+   * Get the channel number.
+   *
+   * @return The channel number.
+   */
+  int GetChannel() const;
+
+  /**
+   * Set the number of averaging bits.
+   *
+   * This sets the number of averaging bits. The actual number of averaged
+   * samples is 2^bits.
+   *
+   * Use averaging to improve the stability of your measurement at the expense
+   * of sampling rate. The averaging is done automatically in the FPGA.
+   *
+   * @param bits Number of bits of averaging.
+   */
+  void SetAverageBits(int bits);
+
+  /**
+   * Get the number of averaging bits previously configured.
+   *
+   * This gets the number of averaging bits from the FPGA. The actual number of
+   * averaged samples is 2^bits. The averaging is done automatically in the
+   * FPGA.
+   *
+   * @return Number of bits of averaging previously configured.
+   */
+  int GetAverageBits() const;
+
+  /**
+   * Set the number of oversample bits.
+   *
+   * This sets the number of oversample bits. The actual number of oversampled
+   * values is 2^bits. Use oversampling to improve the resolution of your
+   * measurements at the expense of sampling rate. The oversampling is done
+   * automatically in the FPGA.
+   *
+   * @param bits Number of bits of oversampling.
+   */
+  void SetOversampleBits(int bits);
+
+  /**
+   * Get the number of oversample bits previously configured.
+   *
+   * This gets the number of oversample bits from the FPGA. The actual number of
+   * oversampled values is 2^bits. The oversampling is done automatically in the
+   * FPGA.
+   *
+   * @return Number of bits of oversampling previously configured.
+   */
+  int GetOversampleBits() const;
+
+  /**
+   * Get the factory scaling least significant bit weight constant.
+   *
+   * Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+   *
+   * @return Least significant bit weight.
+   */
+  int GetLSBWeight() const;
+
+  /**
+   * Get the factory scaling offset constant.
+   *
+   * Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+   *
+   * @return Offset constant.
+   */
+  int GetOffset() const;
+
+  /**
+   * Set the sample rate per channel for all analog channels.
+   *
+   * The maximum rate is 500kS/s divided by the number of channels in use.
+   * This is 62500 samples/s per channel.
+   *
+   * @param samplesPerSecond The number of samples per second.
+   */
+  static void SetSampleRate(double samplesPerSecond);
+
+  /**
+   * Get the current sample rate for all channels
+   *
+   * @return Sample rate.
+   */
+  static double GetSampleRate();
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  void SetSimDevice(HAL_SimDeviceHandle device);
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  int m_channel;
+  hal::Handle<HAL_AnalogInputHandle, HAL_FreeAnalogInputPort> m_port;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/discrete/CounterBase.hpp

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <units/time.h>
+
+namespace frc {
+
+/**
+ * Interface for counting the number of ticks on a digital input channel.
+ *
+ * Encoders, Gear tooth sensors, and counters should all subclass this so it can
+ * be used to build more advanced classes for control and driving.
+ *
+ * All counters will immediately start counting - Reset() them if you need them
+ * to be zeroed before use.
+ */
+class CounterBase {
+ public:
+  enum EncodingType { k1X, k2X, k4X };
+
+  CounterBase() = default;
+  virtual ~CounterBase() = default;
+
+  CounterBase(CounterBase&&) = default;
+  CounterBase& operator=(CounterBase&&) = default;
+
+  virtual int Get() const = 0;
+  virtual void Reset() = 0;
+  virtual units::second_t GetPeriod() const = 0;
+  virtual void SetMaxPeriod(units::second_t maxPeriod) = 0;
+  virtual bool GetStopped() const = 0;
+  virtual bool GetDirection() const = 0;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/discrete/DigitalInput.hpp

@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/DIO.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+
+/**
+ * Class to read a digital input.
+ *
+ * This class will read digital inputs and return the current value on the
+ * channel. Other devices such as encoders, gear tooth sensors, etc. that are
+ * implemented elsewhere will automatically allocate digital inputs and outputs
+ * as required. This class is only for devices like switches etc. that aren't
+ * implemented anywhere else.
+ */
+class DigitalInput : public wpi::Sendable,
+                     public wpi::SendableHelper<DigitalInput> {
+ public:
+  /**
+   * Create an instance of a Digital Input class.
+   *
+   * Creates a digital input given a channel.
+   *
+   * @param channel The DIO channel 0-9 are on-board, 10-25 are on the MXP port
+   */
+  explicit DigitalInput(int channel);
+
+  DigitalInput(DigitalInput&&) = default;
+  DigitalInput& operator=(DigitalInput&&) = default;
+
+  ~DigitalInput() override = default;
+
+  /**
+   * Get the value from a digital input channel.
+   *
+   * Retrieve the value of a single digital input channel from the FPGA.
+   */
+  bool Get() const;
+
+  /**
+   * @return The GPIO channel number that this object represents.
+   */
+  int GetChannel() const;
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  void SetSimDevice(HAL_SimDeviceHandle device);
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  int m_channel;
+  hal::Handle<HAL_DigitalHandle, HAL_FreeDIOPort> m_handle;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/discrete/DigitalOutput.hpp

@@ -0,0 +1,150 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/DIO.h>
+#include <hal/Types.h>
+#include <units/time.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+
+/**
+ * Class to write to digital outputs.
+ *
+ * Write values to the digital output channels. Other devices implemented
+ * elsewhere will allocate channels automatically so for those devices it
+ * shouldn't be done here.
+ */
+class DigitalOutput : public wpi::Sendable,
+                      public wpi::SendableHelper<DigitalOutput> {
+ public:
+  /**
+   * Create an instance of a digital output.
+   *
+   * Create a digital output given a channel.
+   *
+   * @param channel The digital channel 0-9 are on-board, 10-25 are on the MXP
+   *                port
+   */
+  explicit DigitalOutput(int channel);
+
+  DigitalOutput(DigitalOutput&&) = default;
+  DigitalOutput& operator=(DigitalOutput&&) = default;
+
+  ~DigitalOutput() override;
+
+  /**
+   * Set the value of a digital output.
+   *
+   * Set the value of a digital output to either one (true) or zero (false).
+   *
+   * @param value 1 (true) for high, 0 (false) for disabled
+   */
+  void Set(bool value);
+
+  /**
+   * Gets the value being output from the Digital Output.
+   *
+   * @return the state of the digital output.
+   */
+  bool Get() const;
+
+  /**
+   * @return The GPIO channel number that this object represents.
+   */
+  int GetChannel() const;
+
+  /**
+   * Output a single pulse on the digital output line.
+   *
+   * Send a single pulse on the digital output line where the pulse duration is
+   * specified in seconds. Maximum of 65535 microseconds.
+   *
+   * @param pulseLength The pulse length in seconds
+   */
+  void Pulse(units::second_t pulseLength);
+
+  /**
+   * Determine if the pulse is still going.
+   *
+   * Determine if a previously started pulse is still going.
+   */
+  bool IsPulsing() const;
+
+  /**
+   * Change the PWM frequency of the PWM output on a Digital Output line.
+   *
+   * The valid range is from 0.6 Hz to 19 kHz.  The frequency resolution is
+   * logarithmic.
+   *
+   * There is only one PWM frequency for all digital channels.
+   *
+   * @param rate The frequency to output all digital output PWM signals.
+   */
+  void SetPWMRate(double rate);
+
+  /**
+   * Enable a PWM PPS (Pulse Per Second) Output on this line.
+   *
+   * Allocate one of the 6 DO PWM generator resources from this module.
+   *
+   * Supply the duty-cycle to output.
+   *
+   * The resolution of the duty cycle is 8-bit.
+   *
+   * @param dutyCycle The duty-cycle to start generating. [0..1]
+   */
+  void EnablePPS(double dutyCycle);
+
+  /**
+   * Enable a PWM Output on this line.
+   *
+   * Allocate one of the 6 DO PWM generator resources from this module.
+   *
+   * Supply the initial duty-cycle to output so as to avoid a glitch when first
+   * starting.
+   *
+   * The resolution of the duty cycle is 8-bit for low frequencies (1kHz or
+   * less) but is reduced the higher the frequency of the PWM signal is.
+   *
+   * @param initialDutyCycle The duty-cycle to start generating. [0..1]
+   */
+  void EnablePWM(double initialDutyCycle);
+
+  /**
+   * Change this line from a PWM output back to a static Digital Output line.
+   *
+   * Free up one of the 6 DO PWM generator resources that were in use.
+   */
+  void DisablePWM();
+
+  /**
+   * Change the duty-cycle that is being generated on the line.
+   *
+   * The resolution of the duty cycle is 8-bit for low frequencies (1kHz or
+   * less) but is reduced the higher the frequency of the PWM signal is.
+   *
+   * @param dutyCycle The duty-cycle to change to. [0..1]
+   */
+  void UpdateDutyCycle(double dutyCycle);
+
+  /**
+   * Indicates this output is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  void SetSimDevice(HAL_SimDeviceHandle device);
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  int m_channel;
+  hal::Handle<HAL_DigitalHandle, HAL_FreeDIOPort> m_handle;
+  hal::Handle<HAL_DigitalPWMHandle> m_pwmGenerator;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/discrete/PWM.hpp

@@ -0,0 +1,118 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <hal/PWM.h>
+#include <hal/Types.h>
+#include <units/time.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+class AddressableLED;
+
+/**
+ * Class implements the PWM generation in the FPGA.
+ *
+ * The values supplied as arguments for PWM outputs range from -1.0 to 1.0. They
+ * are mapped to the microseconds to keep the pulse high, with a range of 0
+ * (off) to 4096. Changes are immediately sent to the FPGA, and the update
+ * occurs at the next FPGA cycle (5.05ms). There is no delay.
+ */
+class PWM : public wpi::Sendable, public wpi::SendableHelper<PWM> {
+ public:
+  friend class AddressableLED;
+  /**
+   * Represents the output period in microseconds.
+   */
+  enum OutputPeriod {
+    /**
+     * PWM pulses occur every 5 ms
+     */
+    kOutputPeriod_5Ms = 1,
+    /**
+     * PWM pulses occur every 10 ms
+     */
+    kOutputPeriod_10Ms = 2,
+    /**
+     * PWM pulses occur every 20 ms
+     */
+    kOutputPeriod_20Ms = 4
+  };
+
+  /**
+   * Allocate a PWM given a channel number.
+   *
+   * Checks channel value range and allocates the appropriate channel.
+   * The allocation is only done to help users ensure that they don't double
+   * assign channels.
+   *
+   * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the
+   *                MXP port
+   * @param registerSendable If true, adds this instance to SendableRegistry
+   */
+  explicit PWM(int channel, bool registerSendable = true);
+
+  PWM(PWM&&) = default;
+  PWM& operator=(PWM&&) = default;
+
+  /**
+   * Free the PWM channel.
+   *
+   * Free the resource associated with the PWM channel and set the value to 0.
+   */
+  ~PWM() override;
+
+  /**
+   * Set the PWM pulse time directly to the hardware.
+   *
+   * Write a microsecond value to a PWM channel.
+   *
+   * @param time Microsecond PWM value.
+   */
+  void SetPulseTime(units::microsecond_t time);
+
+  /**
+   * Get the PWM pulse time directly from the hardware.
+   *
+   * Read a microsecond value from a PWM channel.
+   *
+   * @return Microsecond PWM control value.
+   */
+  units::microsecond_t GetPulseTime() const;
+
+  /**
+   * Temporarily disables the PWM output. The next set call will re-enable
+   * the output.
+   */
+  void SetDisabled();
+
+  /**
+   * Sets the PWM output period.
+   *
+   * @param mult The output period to apply to this channel
+   */
+  void SetOutputPeriod(OutputPeriod mult);
+
+  int GetChannel() const;
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  void SetSimDevice(HAL_SimDeviceHandle device);
+
+ protected:
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  int m_channel;
+  hal::Handle<HAL_DigitalHandle, HAL_FreePWMPort> m_handle;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/imu/OnboardIMU.hpp

@@ -0,0 +1,131 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+#include <frc/geometry/Quaternion.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Rotation3d.h>
+
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+namespace frc {
+
+/**
+ * SystemCore onboard IMU
+ */
+class OnboardIMU {
+ public:
+  /**
+   * A mount orientation of SystemCore
+   */
+  enum MountOrientation {
+    /** Flat (mounted parallel to the ground). */
+    kFlat,
+    /** Landscape (vertically mounted with long edge of SystemCore parallel to
+       the ground). */
+    kLandscape,
+    /** Portrait (vertically mounted with the short edge of SystemCore parallel
+       to the ground). */
+    kPortrait
+  };
+
+  /**
+   * Constructs a handle to the SystemCore onboard IMU.
+   * @param mountOrientation the mount orientation of SystemCore to determine
+   * yaw.
+   */
+  explicit OnboardIMU(MountOrientation mountOrientation);
+
+  /**
+   * Get the yaw value
+   * @return yaw value
+   */
+  units::radian_t GetYaw();
+
+  /**
+   * Reset the current yaw value to 0. Future reads of the yaw value will be
+   * relative to the current orientation.
+   */
+  void ResetYaw();
+
+  /**
+   * Get the yaw as a Rotation2d.
+   * @return yaw
+   */
+  Rotation2d GetRotation2d();
+
+  /**
+   * Get the 3D orientation as a Rotation3d.
+   * @return 3D orientation
+   */
+  Rotation3d GetRotation3d();
+
+  /**
+   * Get the 3D orientation as a Quaternion.
+   * @return 3D orientation
+   */
+  Quaternion GetQuaternion();
+
+  /**
+   * Get the angle about the X axis of the IMU.
+   * @return angle about the X axis
+   */
+  units::radian_t GetAngleX();
+
+  /**
+   * Get the angle about the Y axis of the IMU.
+   * @return angle about the Y axis
+   */
+  units::radian_t GetAngleY();
+
+  /**
+   * Get the angle about the Z axis of the IMU.
+   * @return angle about the Z axis
+   */
+  units::radian_t GetAngleZ();
+
+  /**
+   * Get the angular rate about the X axis of the IMU.
+   * @return angular rate about the X axis
+   */
+  units::radians_per_second_t GetGyroRateX();
+
+  /**
+   * Get the angular rate about the Y axis of the IMU.
+   * @return angular rate about the Y axis
+   */
+  units::radians_per_second_t GetGyroRateY();
+
+  /**
+   * Get the angular rate about the Z axis of the IMU.
+   * @return angular rate about the Z axis
+   */
+  units::radians_per_second_t GetGyroRateZ();
+
+  /**
+   * Get the acceleration along the X axis of the IMU.
+   * @return acceleration along the X axis
+   */
+  units::meters_per_second_squared_t GetAccelX();
+
+  /**
+   * Get the acceleration along the Z axis of the IMU.
+   * @return acceleration along the Z axis
+   */
+  units::meters_per_second_squared_t GetAccelY();
+
+  /**
+   * Get the acceleration along the Z axis of the IMU.
+   * @return acceleration along the Z axis
+   */
+  units::meters_per_second_squared_t GetAccelZ();
+
+ private:
+  units::radian_t GetYawNoOffset();
+  MountOrientation m_mountOrientation;
+  units::radian_t m_yawOffset{0};
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/led/AddressableLED.hpp

@@ -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.
+
+#pragma once
+
+#include <initializer_list>
+#include <span>
+
+#include <hal/AddressableLED.h>
+#include <hal/AddressableLEDTypes.h>
+#include <hal/Types.h>
+#include <units/time.h>
+
+#include "util/Color.h"
+#include "util/Color8Bit.h"
+
+namespace frc {
+
+/**
+ * A class for driving addressable LEDs, such as WS2812B, WS2815, and NeoPixels.
+ *
+ * Some LEDs use a different color order than the default GRB. The color order
+ * is configurable using SetColorOrder().
+ *
+ * Up to 1024 LEDs may be controlled in total across all AddressableLED
+ * instances. A single global buffer is used for all instances. The start
+ * position used for LED data for the output is set via SetStart() and the
+ * length of the strip is set via SetLength(). Both of these default to zero, so
+ * multiple instances will access the same pixel data unless SetStart() is
+ * called to adjust the starting point.
+ */
+class AddressableLED {
+ public:
+  /**
+   * Order that color data is sent over the wire.
+   */
+  enum ColorOrder {
+    kRGB = HAL_ALED_RGB,  ///< RGB order
+    kRBG = HAL_ALED_RBG,  ///< RBG order
+    kBGR = HAL_ALED_BGR,  ///< BGR order
+    kBRG = HAL_ALED_BRG,  ///< BRG order
+    kGBR = HAL_ALED_GBR,  ///< GBR order
+    kGRB = HAL_ALED_GRB   ///< GRB order. This is the default order.
+  };
+
+  class LEDData : public HAL_AddressableLEDData {
+   public:
+    LEDData() : LEDData(0, 0, 0) {}
+    LEDData(int _r, int _g, int _b) {
+      r = _r;
+      g = _g;
+      b = _b;
+    }
+
+    /**
+     * A helper method to set all values of the LED.
+     *
+     * @param r the r value [0-255]
+     * @param g the g value [0-255]
+     * @param b the b value [0-255]
+     */
+    void SetRGB(int r, int g, int b) {
+      this->r = r;
+      this->g = g;
+      this->b = b;
+    }
+
+    /**
+     * A helper method to set all values of the LED.
+     *
+     * @param h the h value [0-180]
+     * @param s the s value [0-255]
+     * @param v the v value [0-255]
+     */
+    void SetHSV(int h, int s, int v);
+
+    /*
+     * Sets a specific LED in the buffer.
+     *
+     * @param color The color of the LED
+     */
+    void SetLED(const Color& color) {
+      this->r = color.red * 255;
+      this->g = color.green * 255;
+      this->b = color.blue * 255;
+    }
+
+    /*
+     * Sets a specific LED in the buffer.
+     *
+     * @param color The color of the LED
+     */
+    void SetLED(const Color8Bit& color) {
+      this->r = color.red;
+      this->g = color.green;
+      this->b = color.blue;
+    }
+  };
+
+  /**
+   * Constructs a new driver for a specific channel.
+   *
+   * @param channel the output channel to use
+   */
+  explicit AddressableLED(int channel);
+
+  AddressableLED(AddressableLED&&) = default;
+  AddressableLED& operator=(AddressableLED&&) = default;
+
+  /**
+   * Gets the channel for this addressable LED.
+   *
+   * @return channel
+   */
+  int GetChannel() const { return m_channel; }
+
+  /**
+   * Sets the color order for this AddressableLED. The default order is GRB.
+   *
+   * This will take effect on the next call to SetData().
+   *
+   * @param order the color order
+   */
+  void SetColorOrder(ColorOrder order);
+
+  /**
+   * Sets the display start of the LED strip in the global buffer.
+   *
+   * @param start the strip start, in LEDs
+   */
+  void SetStart(int start);
+
+  /**
+   * Gets the display start of the LED strip in the global buffer.
+   *
+   * @return the strip start, in LEDs
+   */
+  int GetStart() const { return m_start; }
+
+  /**
+   * Sets the length of the LED strip.
+   *
+   * @param length the strip length, in LEDs
+   */
+  void SetLength(int length);
+
+  /**
+   * Sets the LED output data. This will write to the global buffer starting at
+   * the location set by SetStart() and up to the length set by SetLength().
+   *
+   * @param ledData the buffer to write
+   */
+  void SetData(std::span<const LEDData> ledData);
+
+  /**
+   * Sets the LED output data. This will write to the global buffer starting at
+   * the location set by SetStart() and up to the length set by SetLength().
+   *
+   * @param ledData the buffer to write
+   */
+  void SetData(std::initializer_list<LEDData> ledData);
+
+  /**
+   * Sets the LED output data at an arbitrary location in the global buffer.
+   *
+   * @param start the start location, in LEDs
+   * @param colorOrder the color order
+   * @param ledData the buffer to write
+   */
+  static void SetGlobalData(int start, ColorOrder colorOrder,
+                            std::span<const LEDData> ledData);
+
+ private:
+  hal::Handle<HAL_AddressableLEDHandle, HAL_FreeAddressableLED> m_handle;
+  int m_channel;
+  int m_start{0};
+  int m_length{0};
+  ColorOrder m_colorOrder{kGRB};
+};
+
+constexpr auto format_as(AddressableLED::ColorOrder order) {
+  return static_cast<int32_t>(order);
+}
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/led/LEDPattern.hpp

@@ -0,0 +1,404 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <span>
+#include <utility>
+
+#include <units/frequency.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+
+#include "frc/AddressableLED.h"
+#include "util/Color.h"
+
+namespace frc {
+
+class LEDPattern {
+ public:
+  /**
+   * A wrapper around a length and an arbitrary reader function that accepts an
+   * LED index and returns data for the LED at that index. This configuration
+   * allows us to abstract over different container types without templating.
+   */
+  class LEDReader {
+   public:
+    LEDReader(std::function<frc::AddressableLED::LEDData(int)> impl,
+              size_t size)
+        : m_impl{std::move(impl)}, m_size{size} {}
+
+    frc::AddressableLED::LEDData operator[](size_t index) const {
+      return m_impl(index);
+    }
+
+    size_t size() const { return m_size; }
+
+   private:
+    std::function<frc::AddressableLED::LEDData(int)> m_impl;
+    size_t m_size;
+  };
+
+  explicit LEDPattern(std::function<void(frc::LEDPattern::LEDReader,
+                                         std::function<void(int, frc::Color)>)>
+                          impl);
+
+  void ApplyTo(LEDReader reader,
+               std::function<void(int, frc::Color)> writer) const;
+
+  /**
+   * Writes the pattern to an LED buffer. Dynamic animations should be called
+   * periodically (such as with a command or with a periodic method) to refresh
+   * the buffer over time.
+   *
+   * This method is intentionally designed to use separate objects for reading
+   * and writing data. By splitting them up, we can easily modify the behavior
+   * of some base pattern to make it scroll, blink, or breathe by intercepting
+   * the data writes to transform their behavior to whatever we like.
+   *
+   * @param data the current data of the LED strip
+   * @param writer data writer for setting new LED colors on the LED strip
+   */
+  void ApplyTo(std::span<frc::AddressableLED::LEDData> data,
+               std::function<void(int, frc::Color)> writer) const;
+
+  /**
+   * Writes the pattern to an LED buffer. Dynamic animations should be called
+   * periodically (such as with a command or with a periodic method) to refresh
+   * the buffer over time.
+   *
+   * This method is intentionally designed to use separate objects for reading
+   * and writing data. By splitting them up, we can easily modify the behavior
+   * of some base pattern to make it scroll, blink, or breathe by intercepting
+   * the data writes to transform their behavior to whatever we like.
+   *
+   * @param data the current data of the LED strip
+   */
+  void ApplyTo(std::span<frc::AddressableLED::LEDData> data) const;
+
+  /**
+   * Creates a pattern with remapped indices.
+   *
+   * @param indexMapper the index mapper
+   * @return the mapped pattern
+   */
+  [[nodiscard]]
+  LEDPattern MapIndex(std::function<size_t(size_t, size_t)> indexMapper);
+
+  /**
+   * Creates a pattern that displays this one in reverse. Scrolling patterns
+   * will scroll in the opposite direction (but at the same speed). It will
+   * treat the end of an LED strip as the start, and the start of the strip as
+   * the end. This can be useful for making ping-pong patterns that travel from
+   * one end of an LED strip to the other, then reverse direction and move back
+   * to the start. This can also be useful when working with LED strips
+   * connected in a serpentine pattern (where the start of one strip is
+   * connected to the end of the previous one).
+   *
+   * @return the reverse pattern
+   */
+  [[nodiscard]]
+  LEDPattern Reversed();
+
+  /**
+   * Creates a pattern that displays this one, but offset by a certain number of
+   * LEDs. The offset pattern will wrap around, if necessary.
+   *
+   * @param offset how many LEDs to offset by
+   * @return the offset pattern
+   */
+  [[nodiscard]]
+  LEDPattern OffsetBy(int offset);
+
+  /**
+   * Creates a pattern that plays this one scrolling up the buffer. The velocity
+   * controls how fast the pattern returns back to its original position, and is
+   * in terms of the length of the LED strip; scrolling across a segment that is
+   * 10 LEDs long will travel twice as fast as on a segment that's only 5 LEDs
+   * long (assuming equal LED density on both segments).
+   */
+  [[nodiscard]]
+  LEDPattern ScrollAtRelativeSpeed(units::hertz_t velocity);
+
+  /**
+   * Creates a pattern that plays this one scrolling up an LED strip. A negative
+   * velocity makes the pattern play in reverse.
+   *
+   * <p>For example, scrolling a pattern at 4 inches per second along an LED
+   * strip with 60 LEDs per meter:
+   *
+   * <pre>
+   *   // LEDs per meter, a known value taken from the spec sheet of our
+   * particular LED strip units::meter_t LED_SPACING = units::meter_t{1 /60.0};
+   *
+   *   frc::LEDPattern rainbow = frc::LEDPattern::Rainbow();
+   *   frc::LEDPattern scrollingRainbow =
+   *     rainbow.ScrollAtAbsoluteSpeed(units::feet_per_second_t{1 / 3.0},
+   * LED_SPACING);
+   * </pre>
+   *
+   * <p>Note that this pattern will scroll <i>faster</i> if applied to a less
+   * dense LED strip (such as 30 LEDs per meter), or <i>slower</i> if applied to
+   * a denser LED strip (such as 120 or 144 LEDs per meter).
+   *
+   * @param velocity how fast the pattern should move along a physical LED strip
+   * @param ledSpacing the distance between adjacent LEDs on the physical LED
+   * strip
+   * @return the scrolling pattern
+   */
+  [[nodiscard]]
+  LEDPattern ScrollAtAbsoluteSpeed(units::meters_per_second_t velocity,
+                                   units::meter_t ledSpacing);
+
+  /**
+   * Creates a pattern that switches between playing this pattern and turning
+   * the entire LED strip off.
+   *
+   * @param onTime how long the pattern should play for, per cycle
+   * @param offTime how long the pattern should be turned off for, per cycle
+   * @return the blinking pattern
+   */
+  [[nodiscard]]
+  LEDPattern Blink(units::second_t onTime, units::second_t offTime);
+
+  /**
+   * Like {@link LEDPattern::Blink(units::second_t)}, but where the
+   * "off" time is exactly equal to the "on" time.
+   *
+   * @param onTime how long the pattern should play for (and be turned off for),
+   * per cycle
+   * @return the blinking pattern
+   */
+  [[nodiscard]]
+  LEDPattern Blink(units::second_t onTime);
+
+  /**
+   * Creates a pattern that blinks this one on and off in sync with a true/false
+   * signal. The pattern will play while the signal outputs {@code true}, and
+   * will turn off while the signal outputs
+   * {@code false}.
+   *
+   * @param signal the signal to synchronize with
+   * @return the blinking pattern
+   */
+  [[nodiscard]]
+  LEDPattern SynchronizedBlink(std::function<bool()> signal);
+
+  /**
+   * Creates a pattern that brightens and dims this one over time. Brightness
+   * follows a sinusoidal pattern.
+   *
+   * @param period how fast the breathing pattern should complete a single cycle
+   * @return the breathing pattern
+   */
+  [[nodiscard]]
+  LEDPattern Breathe(units::second_t period);
+
+  /**
+   * Creates a pattern that plays this pattern overlaid on another. Anywhere
+   * this pattern sets an LED to off (or {@link frc::Color::kBlack}), the base
+   * pattern will be displayed instead.
+   *
+   * @param base the base pattern to overlay on top of
+   * @return the combined overlay pattern
+   */
+  [[nodiscard]]
+  LEDPattern OverlayOn(const LEDPattern& base);
+
+  /**
+   * Creates a pattern that displays outputs as a combination of this pattern
+   * and another. Color values are calculated as the average color of both
+   * patterns; if both patterns set the same LED to the same color, then it is
+   * set to that color, but if one pattern sets to one color and the other
+   * pattern sets it to off, then it will show the color of the first pattern
+   * but at approximately half brightness. This is different from {@link
+   * LEDPattern::OverlayOn(const LEDPattern&)}, which will show the base pattern
+   * at full brightness if the overlay is set to off at that position.
+   *
+   * @param other the pattern to blend with
+   * @return the blended pattern
+   */
+  [[nodiscard]]
+  LEDPattern Blend(const LEDPattern& other);
+
+  /**
+   * Similar to {@link LEDPattern::Blend(const LEDPattern&)}, but performs a
+   * bitwise mask on each color channel rather than averaging the colors for
+   * each LED. This can be helpful for displaying only a portion of the base
+   * pattern by applying a mask that sets the desired area to white, and all
+   * other areas to black. However, it can also be used to display only certain
+   * color channels or hues; for example, masking with {@code
+   * LEDPattern.color(Color.kRed)} will turn off the green and blue channels on
+   * the output pattern, leaving only the red LEDs to be illuminated.
+   *
+   * @param mask the mask to apply
+   * @return the masked pattern
+   */
+  [[nodiscard]]
+  LEDPattern Mask(const LEDPattern& mask);
+
+  /**
+   * Creates a pattern that plays this one, but at a different brightness.
+   * Brightness multipliers are applied per-channel in the RGB space; no HSL or
+   * HSV conversions are applied. Multipliers are also uncapped, which may
+   * result in the original colors washing out and appearing less saturated or
+   * even just a bright white.
+   *
+   * <p>This method is predominantly intended for dimming LEDs to avoid
+   * painfully bright or distracting patterns from playing (apologies to the
+   * 2024 NE Greater Boston field staff).
+   *
+   * <p>For example, dimming can be done simply by adding a call to
+   * `atBrightness` at the end of a pattern:
+   *
+   * <pre>
+   *   // Solid red, but at 50% brightness
+   *   frc::LEDPattern::Solid(frc::Color::kRed).AtBrightness(0.5);
+   *
+   *   // Solid white, but at only 10% (i.e. ~0.5V)
+   *   frc::LEDPattern::Solid(frc:Color::kWhite).AtBrightness(0.1);
+   * </pre>
+   *
+   * @param relativeBrightness the multiplier to apply to all channels to modify
+   * brightness
+   * @return the input pattern, displayed at
+   */
+  [[nodiscard]]
+  LEDPattern AtBrightness(double relativeBrightness);
+
+  /** A pattern that turns off all LEDs. */
+  static LEDPattern Off();
+
+  /**
+   * Creates a pattern that displays a single static color along the entire
+   * length of the LED strip.
+   *
+   * @param color the color to display
+   * @return the pattern
+   */
+  static LEDPattern Solid(const Color color);
+
+  /**
+   * Creates a pattern that works as a mask layer for {@link
+   * LEDPattern::Mask(const LEDPattern&)} that illuminates only the portion of
+   * the LED strip corresponding with some progress. The mask pattern will start
+   * from the base and set LEDs to white at a proportion equal to the progress
+   * returned by the function. Some usages for this could be for displaying
+   * progress of a flywheel to its target velocity, progress of a complex
+   * autonomous sequence, or the height of an elevator.
+   *
+   * <p>For example, creating a mask for displaying a red-to-blue gradient,
+   * starting from the red end, based on where an elevator is in its range of
+   * travel.
+   *
+   * <pre>
+   * frc::LEDPattern basePattern =
+   *   frc::LEDPattern::Gradient(frc::Color::kRed, frc::Color::kBlue);
+   * frc::LEDPattern progressPattern =
+   *   basePattern.Mask(frc::LEDPattern::ProgressMaskLayer([&]() {
+   *     return elevator.GetHeight() / elevator.MaxHeight();
+   *   });
+   * </pre>
+   *
+   * @param progressFunction the function to call to determine the progress.
+   * This should return values in the range [0, 1]; any values outside that
+   * range will be clamped.
+   * @return the mask pattern
+   */
+  static LEDPattern ProgressMaskLayer(std::function<double()> progressFunction);
+
+  /**
+   * Display a set of colors in steps across the length of the LED strip. No
+   * interpolation is done between colors. Colors are specified by the first LED
+   * on the strip to show that color. The last color in the map will be
+   * displayed all the way to the end of the strip. LEDs positioned before the
+   * first specified step will be turned off (you can think of this as if
+   * there's a 0 -> black step by default).
+   *
+   * @param steps a map of progress to the color to start displaying at that
+   * position along the LED strip
+   * @return a motionless step pattern
+   */
+  static LEDPattern Steps(std::span<const std::pair<double, Color>> steps);
+
+  /**
+   * Display a set of colors in steps across the length of the LED strip. No
+   * interpolation is done between colors. Colors are specified by the first LED
+   * on the strip to show that color. The last color in the map will be
+   * displayed all the way to the end of the strip. LEDs positioned before the
+   * first specified step will be turned off (you can think of this as if
+   * there's a 0 -> black step by default).
+   *
+   * @param steps a map of progress to the color to start displaying at that
+   * position along the LED strip
+   * @return a motionless step pattern
+   */
+  static LEDPattern Steps(
+      std::initializer_list<std::pair<double, Color>> steps);
+
+  /** Types of gradients. */
+  enum GradientType {
+    /**
+     * A continuous gradient, where the gradient wraps around to allow for
+     * seamless scrolling effects.
+     */
+    kContinuous,
+    /**
+     * A discontinuous gradient, where the first pixel is set to the first color
+     * of the gradient and the final pixel is set to the last color of the
+     * gradient. There is no wrapping effect, so scrolling effects will display
+     * an obvious seam.
+     */
+    kDiscontinuous
+  };
+
+  /**
+   * Creates a pattern that displays a non-animated gradient of colors across
+   * the entire length of the LED strip. Colors are evenly distributed along the
+   * full length of the LED strip. The gradient type is configured with the
+   * {@code type} parameter, allowing the gradient to be either continuous (no
+   * seams, good for scrolling effects) or discontinuous (a clear seam is
+   * visible, but the gradient applies to the full length of the LED strip
+   * without needing to use some space for wrapping).
+   *
+   * @param type the type of gradient (continuous or discontinuous)
+   * @param colors the colors to display in the gradient
+   * @return a motionless gradient pattern
+   */
+  static LEDPattern Gradient(GradientType type, std::span<const Color> colors);
+
+  /**
+   * Creates a pattern that displays a non-animated gradient of colors across
+   * the entire length of the LED strip. Colors are evenly distributed along the
+   * full length of the LED strip. The gradient type is configured with the
+   * {@code type} parameter, allowing the gradient to be either continuous (no
+   * seams, good for scrolling effects) or discontinuous (a clear seam is
+   * visible, but the gradient applies to the full length of the LED strip
+   * without needing to use some space for wrapping).
+   *
+   * @param type the type of gradient (continuous or discontinuous)
+   * @param colors the colors to display in the gradient
+   * @return a motionless gradient pattern
+   */
+  static LEDPattern Gradient(GradientType type,
+                             std::initializer_list<Color> colors);
+
+  /**
+   * Creates an LED pattern that displays a rainbow across the color wheel. The
+   * rainbow pattern will stretch across the entire length of the LED strip.
+   *
+   * @param saturation the saturation of the HSV colors, in [0, 255]
+   * @param value the value of the HSV colors, in [0, 255]
+   * @return the rainbow pattern
+   */
+  static LEDPattern Rainbow(int saturation, int value);
+
+ private:
+  std::function<void(frc::LEDPattern::LEDReader,
+                     std::function<void(int, frc::Color)>)>
+      m_impl;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/motor/MotorController.hpp

@@ -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.
+
+#pragma once
+
+#include <units/voltage.h>
+
+namespace frc {
+
+/**
+ * Interface for motor controlling devices.
+ */
+class MotorController {
+ public:
+  virtual ~MotorController() = default;
+
+  /**
+   * Common interface for setting the speed of a motor controller.
+   *
+   * @param speed The speed to set.  Value should be between -1.0 and 1.0.
+   */
+  virtual void Set(double speed) = 0;
+
+  /**
+   * Sets the voltage output of the MotorController.  Compensates for
+   * the current bus voltage to ensure that the desired voltage is output even
+   * if the battery voltage is below 12V - highly useful when the voltage
+   * outputs are "meaningful" (e.g. they come from a feedforward calculation).
+   *
+   * <p>NOTE: This function *must* be called regularly in order for voltage
+   * compensation to work properly - unlike the ordinary set function, it is not
+   * "set it and forget it."
+   *
+   * @param output The voltage to output.
+   */
+  virtual void SetVoltage(units::volt_t output);
+
+  /**
+   * Common interface for getting the current set speed of a motor controller.
+   *
+   * @return The current set speed.  Value is between -1.0 and 1.0.
+   */
+  virtual double Get() const = 0;
+
+  /**
+   * Common interface for inverting direction of a motor controller.
+   *
+   * @param isInverted The state of inversion, true is inverted.
+   */
+  virtual void SetInverted(bool isInverted) = 0;
+
+  /**
+   * Common interface for returning the inversion state of a motor controller.
+   *
+   * @return isInverted The state of inversion, true is inverted.
+   */
+  virtual bool GetInverted() const = 0;
+
+  /**
+   * Common interface for disabling a motor.
+   */
+  virtual void Disable() = 0;
+
+  /**
+   * Common interface to stop the motor until Set is called again.
+   */
+  virtual void StopMotor() = 0;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/motor/MotorControllerGroup.hpp

@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+#include <wpi/deprecated.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/motorcontrol/MotorController.h"
+
+WPI_IGNORE_DEPRECATED
+
+namespace frc {
+
+/**
+ * Allows multiple MotorController objects to be linked together.
+ */
+class [[deprecated(
+    "Use PWMMotorController::AddFollower() or if using CAN motor controllers,"
+    "use their method of following.")]] MotorControllerGroup
+    : public wpi::Sendable,
+      public MotorController,
+      public wpi::SendableHelper<MotorControllerGroup> {
+ public:
+  /**
+   * Create a new MotorControllerGroup with the provided MotorControllers.
+   *
+   * @tparam MotorControllers The MotorController types.
+   * @param motorController The first MotorController to add
+   * @param motorControllers The MotorControllers to add
+   */
+  template <class... MotorControllers>
+  explicit MotorControllerGroup(MotorController& motorController,
+                                MotorControllers&... motorControllers)
+      : m_motorControllers(std::vector<std::reference_wrapper<MotorController>>{
+            motorController, motorControllers...}) {
+    Initialize();
+  }
+
+  /**
+   * Create a new MotorControllerGroup with the provided MotorControllers.
+   *
+   * @param motorControllers The MotorControllers to add.
+   */
+  explicit MotorControllerGroup(
+      std::vector<std::reference_wrapper<MotorController>>&& motorControllers);
+
+  MotorControllerGroup(MotorControllerGroup&&) = default;
+  MotorControllerGroup& operator=(MotorControllerGroup&&) = default;
+
+  void Set(double speed) override;
+  void SetVoltage(units::volt_t output) override;
+  double Get() const override;
+  void SetInverted(bool isInverted) override;
+  bool GetInverted() const override;
+  void Disable() override;
+  void StopMotor() override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  bool m_isInverted = false;
+  std::vector<std::reference_wrapper<MotorController>> m_motorControllers;
+
+  void Initialize();
+};
+
+}  // namespace frc
+
+WPI_UNIGNORE_DEPRECATED

wpilibc/src/main/native/include/wpi/hardware/motor/MotorSafety.hpp

@@ -0,0 +1,123 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+
+#include <units/time.h>
+#include <wpi/mutex.h>
+
+#include "frc/Timer.h"
+
+namespace frc {
+
+/**
+ * The Motor Safety feature acts as a watchdog timer for an individual motor. It
+ * operates by maintaining a timer that tracks how long it has been since the
+ * feed() method has been called for that actuator. Code in the Driver Station
+ * class initiates a comparison of these timers to the timeout values for any
+ * actuator with safety enabled every 5 received packets (100ms nominal).
+ *
+ * The subclass should call Feed() whenever the motor value is updated.
+ */
+class MotorSafety {
+ public:
+  MotorSafety();
+  virtual ~MotorSafety();
+
+  MotorSafety(MotorSafety&& rhs);
+  MotorSafety& operator=(MotorSafety&& rhs);
+
+  /**
+   * Feed the motor safety object.
+   *
+   * Resets the timer on this object that is used to do the timeouts.
+   */
+  void Feed();
+
+  /**
+   * Set the expiration time for the corresponding motor safety object.
+   *
+   * @param expirationTime The timeout value.
+   */
+  void SetExpiration(units::second_t expirationTime);
+
+  /**
+   * Retrieve the timeout value for the corresponding motor safety object.
+   *
+   * @return the timeout value.
+   */
+  units::second_t GetExpiration() const;
+
+  /**
+   * Determine if the motor is still operating or has timed out.
+   *
+   * @return true if the motor is still operating normally and hasn't timed out.
+   */
+  bool IsAlive() const;
+
+  /**
+   * Enable/disable motor safety for this device.
+   *
+   * Turn on and off the motor safety option for this PWM object.
+   *
+   * @param enabled True if motor safety is enforced for this object.
+   */
+  void SetSafetyEnabled(bool enabled);
+
+  /**
+   * Return the state of the motor safety enabled flag.
+   *
+   * Return if the motor safety is currently enabled for this device.
+   *
+   * @return True if motor safety is enforced for this device.
+   */
+  bool IsSafetyEnabled() const;
+
+  /**
+   * Check if this motor has exceeded its timeout.
+   *
+   * This method is called periodically to determine if this motor has exceeded
+   * its timeout value. If it has, the stop method is called, and the motor is
+   * shut down until its value is updated again.
+   */
+  void Check();
+
+  /**
+   * Check the motors to see if any have timed out.
+   *
+   * This static method is called periodically to poll all the motors and stop
+   * any that have timed out.
+   */
+  static void CheckMotors();
+
+  /**
+   * Called to stop the motor when the timeout expires.
+   */
+  virtual void StopMotor() = 0;
+
+  /**
+   * Returns a description to print when an error occurs.
+   *
+   * @return Description to print when an error occurs.
+   */
+  virtual std::string GetDescription() const = 0;
+
+ private:
+  static constexpr auto kDefaultSafetyExpiration = 100_ms;
+
+  // The expiration time for this object
+  units::second_t m_expiration = kDefaultSafetyExpiration;
+
+  // True if motor safety is enabled for this motor
+  bool m_enabled = false;
+
+  // The FPGA clock value when the motor has expired
+  units::second_t m_stopTime = Timer::GetFPGATimestamp();
+
+  mutable wpi::mutex m_thisMutex;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/motor/PWMMotorController.hpp

@@ -0,0 +1,171 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+#include <memory>
+#include <string>
+#include <string_view>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+#include <hal/SimDevice.h>
+#include <units/voltage.h>
+#include <wpi/deprecated.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/MotorSafety.h"
+#include "frc/PWM.h"
+#include "frc/motorcontrol/MotorController.h"
+
+namespace frc {
+
+WPI_IGNORE_DEPRECATED
+
+/**
+ * Common base class for all PWM Motor Controllers.
+ */
+class PWMMotorController : public MotorController,
+                           public MotorSafety,
+                           public wpi::Sendable,
+                           public wpi::SendableHelper<PWMMotorController> {
+ public:
+  PWMMotorController(PWMMotorController&&) = default;
+  PWMMotorController& operator=(PWMMotorController&&) = default;
+
+  /**
+   * Set the PWM value.
+   *
+   * The PWM value is set using a range of -1.0 to 1.0, appropriately scaling
+   * the value for the FPGA.
+   *
+   * @param value The speed value between -1.0 and 1.0 to set.
+   */
+  void Set(double value) override;
+
+  /**
+   * Sets the voltage output of the PWMMotorController. Compensates for
+   * the current bus voltage to ensure that the desired voltage is output even
+   * if the battery voltage is below 12V - highly useful when the voltage
+   * outputs are "meaningful" (e.g. they come from a feedforward calculation).
+   *
+   * <p>NOTE: This function *must* be called regularly in order for voltage
+   * compensation to work properly - unlike the ordinary set function, it is not
+   * "set it and forget it."
+   *
+   * @param output The voltage to output.
+   */
+  void SetVoltage(units::volt_t output) override;
+
+  /**
+   * Get the recently set value of the PWM. This value is affected by the
+   * inversion property. If you want the value that is sent directly to the
+   * MotorController, use PWM::GetSpeed() instead.
+   *
+   * @return The most recently set value for the PWM between -1.0 and 1.0.
+   */
+  double Get() const override;
+
+  /**
+   * Gets the voltage output of the motor controller, nominally between -12 V
+   * and 12 V.
+   *
+   * @return The voltage of the motor controller, nominally between -12 V and 12
+   *   V.
+   */
+  virtual units::volt_t GetVoltage() const;
+
+  void SetInverted(bool isInverted) override;
+
+  bool GetInverted() const override;
+
+  void Disable() override;
+
+  // MotorSafety interface
+  void StopMotor() override;
+  std::string GetDescription() const override;
+
+  int GetChannel() const;
+
+  /**
+   * Optionally eliminate the deadband from a motor controller.
+   *
+   * @param eliminateDeadband If true, set the motor curve on the motor
+   *                          controller to eliminate the deadband in the middle
+   *                          of the range. Otherwise, keep the full range
+   *                          without modifying any values.
+   */
+  void EnableDeadbandElimination(bool eliminateDeadband);
+
+  /**
+   * Make the given PWM motor controller follow the output of this one.
+   *
+   * @param follower The motor controller follower.
+   */
+  void AddFollower(PWMMotorController& follower);
+
+  /**
+   * Make the given PWM motor controller follow the output of this one.
+   *
+   * @param follower The motor controller follower.
+   */
+  template <std::derived_from<PWMMotorController> T>
+  void AddFollower(T&& follower) {
+    m_owningFollowers.emplace_back(
+        std::make_unique<std::decay_t<T>>(std::forward<T>(follower)));
+  }
+
+ protected:
+  /**
+   * Constructor for a PWM Motor %Controller connected via PWM.
+   *
+   * @param name Name to use for SendableRegistry
+   * @param channel The PWM channel that the controller is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  PWMMotorController(std::string_view name, int channel);
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+  /// PWM instances for motor controller.
+  PWM m_pwm;
+
+  void SetSpeed(double speed);
+  double GetSpeed() const;
+
+  void SetBounds(units::microsecond_t maxPwm,
+                 units::microsecond_t deadbandMaxPwm,
+                 units::microsecond_t centerPwm,
+                 units::microsecond_t deadbandMinPwm,
+                 units::microsecond_t minPwm);
+
+ private:
+  bool m_isInverted = false;
+  std::vector<PWMMotorController*> m_nonowningFollowers;
+  std::vector<std::unique_ptr<PWMMotorController>> m_owningFollowers;
+
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simSpeed;
+
+  bool m_eliminateDeadband{0};
+  units::microsecond_t m_minPwm{0};
+  units::microsecond_t m_deadbandMinPwm{0};
+  units::microsecond_t m_centerPwm{0};
+  units::microsecond_t m_deadbandMaxPwm{0};
+  units::microsecond_t m_maxPwm{0};
+
+  units::microsecond_t GetMinPositivePwm() const;
+  units::microsecond_t GetMaxNegativePwm() const;
+  units::microsecond_t GetPositiveScaleFactor() const;
+  units::microsecond_t GetNegativeScaleFactor() const;
+
+  PWM* GetPwm() { return &m_pwm; }
+};
+
+WPI_UNIGNORE_DEPRECATED
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/pneumatic/Compressor.hpp

@@ -0,0 +1,183 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/CompressorConfigType.h"
+#include "frc/PneumaticsBase.h"
+#include "frc/PneumaticsModuleType.h"
+#include "frc/SensorUtil.h"
+
+namespace frc {
+
+/**
+ * Class for operating a compressor connected to a pneumatics module.
+ *
+ * The module will automatically run in closed loop mode by default whenever a
+ * Solenoid object is created. For most cases, a Compressor object does not need
+ * to be instantiated or used in a robot program. This class is only required in
+ * cases where the robot program needs a more detailed status of the compressor
+ * or to enable/disable closed loop control.
+ *
+ * Note: you cannot operate the compressor directly from this class as doing so
+ * would circumvent the safety provided by using the pressure switch and closed
+ * loop control. You can only turn off closed loop control, thereby stopping
+ * the compressor from operating.
+ */
+class Compressor : public wpi::Sendable,
+                   public wpi::SendableHelper<Compressor> {
+ public:
+  /**
+   * Constructs a compressor for a specified module and type.
+   *
+   * @param busId The bus ID.
+   * @param module The module ID to use.
+   * @param moduleType The module type to use.
+   */
+  Compressor(int busId, int module, PneumaticsModuleType moduleType);
+
+  /**
+   * Constructs a compressor for a default module and specified type.
+   *
+   * @param busId The bus ID.
+   * @param moduleType The module type to use.
+   */
+  Compressor(int busId, PneumaticsModuleType moduleType);
+
+  ~Compressor() override;
+
+  Compressor(const Compressor&) = delete;
+  Compressor& operator=(const Compressor&) = delete;
+
+  Compressor(Compressor&&) = default;
+  Compressor& operator=(Compressor&&) = default;
+
+  /**
+   * Returns whether the compressor is active or not.
+   *
+   * @return true if the compressor is on - otherwise false.
+   */
+  bool IsEnabled() const;
+
+  /**
+   * Returns the state of the pressure switch.
+   *
+   * @return True if pressure switch indicates that the system is not full,
+   * otherwise false.
+   */
+  bool GetPressureSwitchValue() const;
+
+  /**
+   * Get the current drawn by the compressor.
+   *
+   * @return Current drawn by the compressor.
+   */
+  units::ampere_t GetCurrent() const;
+
+  /**
+   * If supported by the device, returns the analog input voltage (on channel
+   * 0).
+   *
+   * This function is only supported by the REV PH. On CTRE PCM, this will
+   * return 0.
+   *
+   * @return The analog input voltage, in volts.
+   */
+  units::volt_t GetAnalogVoltage() const;
+
+  /**
+   * If supported by the device, returns the pressure read by the analog
+   * pressure sensor (on channel 0).
+   *
+   * This function is only supported by the REV PH with the REV Analog Pressure
+   * Sensor. On CTRE PCM, this will return 0.
+   *
+   * @return The pressure read by the analog pressure sensor.
+   */
+  units::pounds_per_square_inch_t GetPressure() const;
+
+  /**
+   * Disable the compressor.
+   */
+  void Disable();
+
+  /**
+   * Enables the compressor in digital mode using the digital pressure switch.
+   * The compressor will turn on when the pressure switch indicates that the
+   * system is not full, and will turn off when the pressure switch indicates
+   * that the system is full.
+   */
+  void EnableDigital();
+
+  /**
+   * If supported by the device, enables the compressor in analog mode. This
+   * mode uses an analog pressure sensor connected to analog channel 0 to cycle
+   * the compressor. The compressor will turn on when the pressure drops below
+   * {@code minPressure} and will turn off when the pressure reaches {@code
+   * maxPressure}. This mode is only supported by the REV PH with the REV Analog
+   * Pressure Sensor connected to analog channel 0.
+   *
+   * On CTRE PCM, this will enable digital control.
+   *
+   * @param minPressure The minimum pressure. The compressor will turn on when
+   * the pressure drops below this value.
+   * @param maxPressure The maximum pressure. The compressor will turn off when
+   * the pressure reaches this value.
+   */
+  void EnableAnalog(units::pounds_per_square_inch_t minPressure,
+                    units::pounds_per_square_inch_t maxPressure);
+
+  /**
+   * If supported by the device, enables the compressor in hybrid mode. This
+   * mode uses both a digital pressure switch and an analog pressure sensor
+   * connected to analog channel 0 to cycle the compressor. This mode is only
+   * supported by the REV PH with the REV Analog Pressure Sensor connected to
+   * analog channel 0.
+   *
+   * The compressor will turn on when \a both:
+   *
+   * - The digital pressure switch indicates the system is not full AND
+   * - The analog pressure sensor indicates that the pressure in the system
+   * is below the specified minimum pressure.
+   *
+   * The compressor will turn off when \a either:
+   *
+   * - The digital pressure switch is disconnected or indicates that the system
+   * is full OR
+   * - The pressure detected by the analog sensor is greater than the specified
+   * maximum pressure.
+   *
+   * On CTRE PCM, this will enable digital control.
+   *
+   * @param minPressure The minimum pressure. The compressor will turn on
+   * when the pressure drops below this value and the pressure switch indicates
+   * that the system is not full.
+   * @param maxPressure The maximum pressure. The compressor will turn
+   * off when the pressure reaches this value or the pressure switch is
+   * disconnected or indicates that the system is full.
+   */
+  void EnableHybrid(units::pounds_per_square_inch_t minPressure,
+                    units::pounds_per_square_inch_t maxPressure);
+
+  /**
+   * Returns the active compressor configuration.
+   *
+   * @return The active compressor configuration.
+   */
+  CompressorConfigType GetConfigType() const;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  std::shared_ptr<PneumaticsBase> m_module;
+  PneumaticsModuleType m_moduleType;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/pneumatic/CompressorConfigType.hpp

@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace frc {
+/**
+ * Compressor config type.
+ */
+enum class CompressorConfigType {
+  /// Disabled.
+  Disabled = 0,
+  /// Digital.
+  Digital = 1,
+  /// Analog.
+  Analog = 2,
+  /// Hybrid.
+  Hybrid = 3
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/pneumatic/DoubleSolenoid.hpp

@@ -0,0 +1,140 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/PneumaticsBase.h"
+#include "frc/PneumaticsModuleType.h"
+
+namespace frc {
+
+/**
+ * DoubleSolenoid class for running 2 channels of high voltage Digital Output
+ * on a pneumatics module.
+ *
+ * The DoubleSolenoid class is typically used for pneumatics solenoids that
+ * have two positions controlled by two separate channels.
+ */
+class DoubleSolenoid : public wpi::Sendable,
+                       public wpi::SendableHelper<DoubleSolenoid> {
+ public:
+  /**
+   * Possible values for a DoubleSolenoid.
+   */
+  enum Value {
+    /// Off position.
+    kOff,
+    /// Forward position.
+    kForward,
+    /// Reverse position.
+    kReverse
+  };
+
+  /**
+   * Constructs a double solenoid for a specified module of a specific module
+   * type.
+   *
+   * @param busId The bus ID.
+   * @param module The module of the solenoid module to use.
+   * @param moduleType The module type to use.
+   * @param forwardChannel The forward channel on the module to control.
+   * @param reverseChannel The reverse channel on the module to control.
+   */
+  DoubleSolenoid(int busId, int module, PneumaticsModuleType moduleType,
+                 int forwardChannel, int reverseChannel);
+
+  /**
+   * Constructs a double solenoid for a default module of a specific module
+   * type.
+   *
+   * @param busId The bus ID.
+   * @param moduleType The module type to use.
+   * @param forwardChannel The forward channel on the module to control.
+   * @param reverseChannel The reverse channel on the module to control.
+   */
+  DoubleSolenoid(int busId, PneumaticsModuleType moduleType, int forwardChannel,
+                 int reverseChannel);
+
+  ~DoubleSolenoid() override;
+
+  DoubleSolenoid(DoubleSolenoid&&) = default;
+  DoubleSolenoid& operator=(DoubleSolenoid&&) = default;
+
+  /**
+   * Set the value of a solenoid.
+   *
+   * @param value The value to set (Off, Forward or Reverse)
+   */
+  void Set(Value value);
+
+  /**
+   * Read the current value of the solenoid.
+   *
+   * @return The current value of the solenoid.
+   */
+  Value Get() const;
+
+  /**
+   * Toggle the value of the solenoid.
+   *
+   * If the solenoid is set to forward, it'll be set to reverse. If the solenoid
+   * is set to reverse, it'll be set to forward. If the solenoid is set to off,
+   * nothing happens.
+   */
+  void Toggle();
+
+  /**
+   * Get the forward channel.
+   *
+   * @return the forward channel.
+   */
+  int GetFwdChannel() const;
+
+  /**
+   * Get the reverse channel.
+   *
+   * @return the reverse channel.
+   */
+  int GetRevChannel() const;
+
+  /**
+   * Check if the forward solenoid is Disabled.
+   *
+   * If a solenoid is shorted, it is added to the DisabledList and disabled
+   * until power cycle, or until faults are cleared.
+   *
+   * @see ClearAllStickyFaults()
+   * @return If solenoid is disabled due to short.
+   */
+  bool IsFwdSolenoidDisabled() const;
+
+  /**
+   * Check if the reverse solenoid is Disabled.
+   *
+   * If a solenoid is shorted, it is added to the DisabledList and disabled
+   * until power cycle, or until faults are cleared.
+   *
+   * @see ClearAllStickyFaults()
+   * @return If solenoid is disabled due to short.
+   */
+  bool IsRevSolenoidDisabled() const;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  std::shared_ptr<PneumaticsBase> m_module;
+  int m_forwardChannel;  // The forward channel on the module to control.
+  int m_reverseChannel;  // The reverse channel on the module to control.
+  int m_forwardMask;     // The mask for the forward channel.
+  int m_reverseMask;     // The mask for the reverse channel.
+  int m_mask;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/pneumatic/PneumaticHub.hpp

@@ -0,0 +1,317 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+#include <units/pressure.h>
+#include <wpi/DenseMap.h>
+#include <wpi/mutex.h>
+
+#include "PneumaticsBase.h"
+
+namespace frc {
+/** Module class for controlling a REV Robotics Pneumatic Hub. */
+class PneumaticHub : public PneumaticsBase {
+ public:
+  /**
+   * Constructs a PneumaticHub with the default ID (1).
+   *
+   * @param busId The bus ID.
+   */
+  explicit PneumaticHub(int busId);
+
+  /**
+   * Constructs a PneumaticHub.
+   *
+   * @param busId The bus ID.
+   * @param module module number to construct
+   */
+  PneumaticHub(int busId, int module);
+
+  ~PneumaticHub() override = default;
+
+  bool GetCompressor() const override;
+
+  /**
+   * Disables the compressor. The compressor will not turn on until
+   * EnableCompressorDigital(), EnableCompressorAnalog(), or
+   * EnableCompressorHybrid() are called.
+   */
+  void DisableCompressor() override;
+
+  void EnableCompressorDigital() override;
+
+  /**
+   * Enables the compressor in analog mode. This mode uses an analog pressure
+   * sensor connected to analog channel 0 to cycle the compressor. The
+   * compressor will turn on when the pressure drops below {@code minPressure}
+   * and will turn off when the pressure reaches {@code maxPressure}.
+   *
+   * @param minPressure The minimum pressure. The compressor will turn on when
+   * the pressure drops below this value. Range 0 - 120 PSI.
+   * @param maxPressure The maximum pressure. The compressor will turn off when
+   * the pressure reaches this value. Range 0 - 120 PSI. Must be larger then
+   * minPressure.
+   */
+  void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
+
+  /**
+   * Enables the compressor in hybrid mode. This mode uses both a digital
+   * pressure switch and an analog pressure sensor connected to analog channel 0
+   * to cycle the compressor.
+   *
+   * The compressor will turn on when \a both:
+   *
+   * - The digital pressure switch indicates the system is not full AND
+   * - The analog pressure sensor indicates that the pressure in the system is
+   * below the specified minimum pressure.
+   *
+   * The compressor will turn off when \a either:
+   *
+   * - The digital pressure switch is disconnected or indicates that the system
+   * is full OR
+   * - The pressure detected by the analog sensor is greater than the specified
+   * maximum pressure.
+   *
+   * @param minPressure The minimum pressure. The compressor will turn on when
+   * the pressure drops below this value and the pressure switch indicates that
+   * the system is not full.  Range 0 - 120 PSI.
+   * @param maxPressure The maximum pressure. The compressor will turn off when
+   * the pressure reaches this value or the pressure switch is disconnected or
+   * indicates that the system is full. Range 0 - 120 PSI. Must be larger then
+   * minPressure.
+   */
+  void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
+
+  CompressorConfigType GetCompressorConfigType() const override;
+
+  bool GetPressureSwitch() const override;
+
+  units::ampere_t GetCompressorCurrent() const override;
+
+  void SetSolenoids(int mask, int values) override;
+
+  int GetSolenoids() const override;
+
+  int GetModuleNumber() const override;
+
+  int GetSolenoidDisabledList() const override;
+
+  void FireOneShot(int index) override;
+
+  void SetOneShotDuration(int index, units::second_t duration) override;
+
+  bool CheckSolenoidChannel(int channel) const override;
+
+  int CheckAndReserveSolenoids(int mask) override;
+
+  void UnreserveSolenoids(int mask) override;
+
+  bool ReserveCompressor() override;
+
+  void UnreserveCompressor() override;
+
+  Solenoid MakeSolenoid(int channel) override;
+  DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
+                                    int reverseChannel) override;
+  Compressor MakeCompressor() override;
+
+  void ReportUsage(std::string_view device, std::string_view data) override;
+
+  /** Version and device data received from a REV PH. */
+  struct Version {
+    /** The firmware major version. */
+    uint32_t FirmwareMajor;
+    /** The firmware minor version. */
+    uint32_t FirmwareMinor;
+    /** The firmware fix version. */
+    uint32_t FirmwareFix;
+    /** The hardware minor version. */
+    uint32_t HardwareMinor;
+    /** The hardware major version. */
+    uint32_t HardwareMajor;
+    /** The device's unique ID. */
+    uint32_t UniqueId;
+  };
+
+  /**
+   * Returns the hardware and firmware versions of this device.
+   *
+   * @return The hardware and firmware versions.
+   */
+  Version GetVersion() const;
+
+  /**
+   * Faults for a REV PH. These faults are only active while the condition is
+   * active.
+   */
+  struct Faults {
+    /** Fault on channel 0. */
+    uint32_t Channel0Fault : 1;
+    /** Fault on channel 1. */
+    uint32_t Channel1Fault : 1;
+    /** Fault on channel 2. */
+    uint32_t Channel2Fault : 1;
+    /** Fault on channel 3. */
+    uint32_t Channel3Fault : 1;
+    /** Fault on channel 4. */
+    uint32_t Channel4Fault : 1;
+    /** Fault on channel 5. */
+    uint32_t Channel5Fault : 1;
+    /** Fault on channel 6. */
+    uint32_t Channel6Fault : 1;
+    /** Fault on channel 7. */
+    uint32_t Channel7Fault : 1;
+    /** Fault on channel 8. */
+    uint32_t Channel8Fault : 1;
+    /** Fault on channel 9. */
+    uint32_t Channel9Fault : 1;
+    /** Fault on channel 10. */
+    uint32_t Channel10Fault : 1;
+    /** Fault on channel 11. */
+    uint32_t Channel11Fault : 1;
+    /** Fault on channel 12. */
+    uint32_t Channel12Fault : 1;
+    /** Fault on channel 13. */
+    uint32_t Channel13Fault : 1;
+    /** Fault on channel 14. */
+    uint32_t Channel14Fault : 1;
+    /** Fault on channel 15. */
+    uint32_t Channel15Fault : 1;
+    /** An overcurrent event occurred on the compressor output. */
+    uint32_t CompressorOverCurrent : 1;
+    /** The compressor output has an open circuit. */
+    uint32_t CompressorOpen : 1;
+    /** An overcurrent event occurred on a solenoid output. */
+    uint32_t SolenoidOverCurrent : 1;
+    /** The input voltage is below the minimum voltage. */
+    uint32_t Brownout : 1;
+    /** A warning was raised by the device's CAN controller. */
+    uint32_t CanWarning : 1;
+    /** The hardware on the device has malfunctioned. */
+    uint32_t HardwareFault : 1;
+
+    /**
+     * Gets whether there is a fault at the specified channel.
+     * @param channel Channel to check for faults.
+     * @return True if a a fault exists at the channel, otherwise false.
+     * @throws A ChannelIndexOutOfRange error if the provided channel is outside
+     * of the range supported by the hardware.
+     */
+    bool GetChannelFault(int channel) const;
+  };
+
+  /**
+   * Returns the faults currently active on this device.
+   *
+   * @return The faults.
+   */
+  Faults GetFaults() const;
+
+  /**
+   * Sticky faults for a REV PH. These faults will remain active until they
+   * are reset by the user.
+   */
+  struct StickyFaults {
+    /** An overcurrent event occurred on the compressor output. */
+    uint32_t CompressorOverCurrent : 1;
+    /** The compressor output has an open circuit. */
+    uint32_t CompressorOpen : 1;
+    /** An overcurrent event occurred on a solenoid output. */
+    uint32_t SolenoidOverCurrent : 1;
+    /** The input voltage is below the minimum voltage. */
+    uint32_t Brownout : 1;
+    /** A warning was raised by the device's CAN controller. */
+    uint32_t CanWarning : 1;
+    /** The device's CAN controller experienced a "Bus Off" event. */
+    uint32_t CanBusOff : 1;
+    /** The hardware on the device has malfunctioned. */
+    uint32_t HardwareFault : 1;
+    /** The firmware on the device has malfunctioned. */
+    uint32_t FirmwareFault : 1;
+    /** The device has rebooted. */
+    uint32_t HasReset : 1;
+  };
+
+  /**
+   * Returns the sticky faults currently active on this device.
+   *
+   * @return The sticky faults.
+   */
+  StickyFaults GetStickyFaults() const;
+
+  /** Clears the sticky faults. */
+  void ClearStickyFaults();
+
+  /**
+   * Returns the current input voltage for this device.
+   *
+   * @return The input voltage.
+   */
+  units::volt_t GetInputVoltage() const;
+
+  /**
+   * Returns the current voltage of the regulated 5v supply.
+   *
+   * @return The current voltage of the 5v supply.
+   */
+  units::volt_t Get5VRegulatedVoltage() const;
+
+  /**
+   * Returns the total current drawn by all solenoids.
+   *
+   * @return Total current drawn by all solenoids.
+   */
+  units::ampere_t GetSolenoidsTotalCurrent() const;
+
+  /**
+   * Returns the current voltage of the solenoid power supply.
+   *
+   * @return The current voltage of the solenoid power supply.
+   */
+  units::volt_t GetSolenoidsVoltage() const;
+
+  /**
+   * Returns the raw voltage of the specified analog input channel.
+   *
+   * @param channel The analog input channel to read voltage from.
+   * @return The voltage of the specified analog input channel.
+   */
+  units::volt_t GetAnalogVoltage(int channel) const override;
+
+  /**
+   * Returns the pressure read by an analog pressure sensor on the specified
+   * analog input channel.
+   *
+   * @param channel The analog input channel to read pressure from.
+   * @return The pressure read by an analog pressure sensor on the specified
+   * analog input channel.
+   */
+  units::pounds_per_square_inch_t GetPressure(int channel) const override;
+
+ private:
+  class DataStore;
+  friend class DataStore;
+  friend class PneumaticsBase;
+  PneumaticHub(int busId, HAL_REVPHHandle handle, int module);
+
+  static std::shared_ptr<PneumaticsBase> GetForModule(int busId, int module);
+
+  std::shared_ptr<DataStore> m_dataStore;
+  HAL_REVPHHandle m_handle;
+  int m_module;
+
+  static wpi::mutex m_handleLock;
+  static std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<DataStore>>[]>
+      m_handleMaps;
+  static std::weak_ptr<DataStore>& GetDataStore(int busId, int module);
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/pneumatic/PneumaticsBase.hpp

@@ -0,0 +1,286 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string_view>
+
+#include <units/current.h>
+#include <units/pressure.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+#include "frc/CompressorConfigType.h"
+#include "frc/PneumaticsModuleType.h"
+
+namespace frc {
+class Solenoid;
+class DoubleSolenoid;
+class Compressor;
+
+/**
+ * Base class for pneumatics devices.
+ */
+class PneumaticsBase {
+ public:
+  virtual ~PneumaticsBase() = default;
+
+  /**
+   * Returns whether the compressor is active or not.
+   *
+   * @return True if the compressor is on - otherwise false.
+   */
+  virtual bool GetCompressor() const = 0;
+
+  /**
+   * Returns the state of the pressure switch.
+   *
+   * @return True if pressure switch indicates that the system is full,
+   * otherwise false.
+   */
+  virtual bool GetPressureSwitch() const = 0;
+
+  /**
+   * Returns the current drawn by the compressor.
+   *
+   * @return The current drawn by the compressor.
+   */
+  virtual units::ampere_t GetCompressorCurrent() const = 0;
+
+  /** Disables the compressor. */
+  virtual void DisableCompressor() = 0;
+
+  /**
+   * Enables the compressor in digital mode using the digital pressure switch.
+   * The compressor will turn on when the pressure switch indicates that the
+   * system is not full, and will turn off when the pressure switch indicates
+   * that the system is full.
+   */
+  virtual void EnableCompressorDigital() = 0;
+
+  /**
+   * If supported by the device, enables the compressor in analog mode. This
+   * mode uses an analog pressure sensor connected to analog channel 0 to cycle
+   * the compressor. The compressor will turn on when the pressure drops below
+   * {@code minPressure} and will turn off when the pressure reaches {@code
+   * maxPressure}. This mode is only supported by the REV PH with the REV Analog
+   * Pressure Sensor connected to analog channel 0.
+   *
+   * On CTRE PCM, this will enable digital control.
+   *
+   * @param minPressure The minimum pressure. The compressor will turn on
+   * when the pressure drops below this value.
+   * @param maxPressure The maximum pressure. The compressor will turn
+   * off when the pressure reaches this value.
+   */
+  virtual void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) = 0;
+
+  /**
+   * If supported by the device, enables the compressor in hybrid mode. This
+   * mode uses both a digital pressure switch and an analog pressure sensor
+   * connected to analog channel 0 to cycle the compressor. This mode is only
+   * supported by the REV PH with the REV Analog Pressure Sensor connected to
+   * analog channel 0.
+   *
+   * The compressor will turn on when \a both:
+   *
+   * - The digital pressure switch indicates the system is not full AND
+   * - The analog pressure sensor indicates that the pressure in the system
+   * is below the specified minimum pressure.
+   *
+   * The compressor will turn off when \a either:
+   *
+   * - The digital pressure switch is disconnected or indicates that the system
+   * is full OR
+   * - The pressure detected by the analog sensor is greater than the specified
+   * maximum pressure.
+   *
+   * On CTRE PCM, this will enable digital control.
+   *
+   * @param minPressure The minimum pressure. The compressor will turn on
+   * when the pressure drops below this value and the pressure switch indicates
+   * that the system is not full.
+   * @param maxPressure The maximum pressure. The compressor will turn
+   * off when the pressure reaches this value or the pressure switch is
+   * disconnected or indicates that the system is full.
+   */
+  virtual void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) = 0;
+
+  /**
+   * Returns the active compressor configuration.
+   *
+   * @return The active compressor configuration.
+   */
+  virtual CompressorConfigType GetCompressorConfigType() const = 0;
+
+  /**
+   * Sets solenoids on a pneumatics module.
+   *
+   * @param mask Bitmask indicating which solenoids to set. The LSB represents
+   * solenoid 0.
+   * @param values Bitmask indicating the desired states of the solenoids. The
+   * LSB represents solenoid 0.
+   */
+  virtual void SetSolenoids(int mask, int values) = 0;
+
+  /**
+   * Gets a bitmask of solenoid values.
+   *
+   * @return Bitmask containing the state of the solenoids. The LSB represents
+   * solenoid 0.
+   */
+  virtual int GetSolenoids() const = 0;
+
+  /**
+   * Get module number for this module.
+   *
+   * @return module number
+   */
+  virtual int GetModuleNumber() const = 0;
+
+  /**
+   * Get a bitmask of disabled solenoids.
+   *
+   * @return Bitmask indicating disabled solenoids. The LSB represents solenoid
+   * 0.
+   */
+  virtual int GetSolenoidDisabledList() const = 0;
+
+  /**
+   * Fire a single solenoid shot.
+   *
+   * @param index solenoid index
+   */
+  virtual void FireOneShot(int index) = 0;
+
+  /**
+   * Set the duration for a single solenoid shot.
+   *
+   * @param index solenoid index
+   * @param duration shot duration
+   */
+  virtual void SetOneShotDuration(int index, units::second_t duration) = 0;
+
+  /**
+   * Check if a solenoid channel is valid.
+   *
+   * @param channel Channel to check
+   * @return True if channel exists
+   */
+  virtual bool CheckSolenoidChannel(int channel) const = 0;
+
+  /**
+   * Check to see if the solenoids marked in the bitmask can be reserved, and if
+   * so, reserve them.
+   *
+   * @param mask The bitmask of solenoids to reserve. The LSB represents
+   * solenoid 0.
+   * @return 0 if successful; mask of solenoids that couldn't be allocated
+   * otherwise
+   */
+  virtual int CheckAndReserveSolenoids(int mask) = 0;
+
+  /**
+   * Unreserve the solenoids marked in the bitmask.
+   *
+   * @param mask The bitmask of solenoids to unreserve. The LSB represents
+   * solenoid 0.
+   */
+  virtual void UnreserveSolenoids(int mask) = 0;
+
+  /**
+   * Reserve the compressor.
+   *
+   * @return true if successful; false if compressor already reserved
+   */
+  virtual bool ReserveCompressor() = 0;
+
+  /**
+   * Unreserve the compressor.
+   */
+  virtual void UnreserveCompressor() = 0;
+
+  /**
+   * If supported by the device, returns the raw voltage of the specified analog
+   * input channel.
+   *
+   * This function is only supported by the REV PH. On CTRE PCM, this will
+   * return 0.
+   *
+   * @param channel The analog input channel to read voltage from.
+   * @return The voltage of the specified analog input channel.
+   */
+  virtual units::volt_t GetAnalogVoltage(int channel) const = 0;
+
+  /**
+   * If supported by the device, returns the pressure read by an analog
+   * pressure sensor on the specified analog input channel.
+   *
+   * This function is only supported by the REV PH. On CTRE PCM, this will
+   * return 0.
+   *
+   * @param channel The analog input channel to read pressure from.
+   * @return The pressure read by an analog pressure sensor on the
+   * specified analog input channel.
+   */
+  virtual units::pounds_per_square_inch_t GetPressure(int channel) const = 0;
+
+  /**
+   * Create a solenoid object for the specified channel.
+   *
+   * @param channel solenoid channel
+   * @return Solenoid object
+   */
+  virtual Solenoid MakeSolenoid(int channel) = 0;
+
+  /**
+   * Create a double solenoid object for the specified channels.
+   *
+   * @param forwardChannel solenoid channel for forward
+   * @param reverseChannel solenoid channel for reverse
+   * @return DoubleSolenoid object
+   */
+  virtual DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
+                                            int reverseChannel) = 0;
+
+  /**
+   * Create a compressor object.
+   *
+   * @return Compressor object
+   */
+  virtual Compressor MakeCompressor() = 0;
+
+  /**
+   * Report usage.
+   *
+   * @param device device and channel as appropriate
+   * @param data arbitrary usage data
+   */
+  virtual void ReportUsage(std::string_view device, std::string_view data) = 0;
+
+  /**
+   * For internal use to get a module for a specific type.
+   *
+   * @param busId The bus ID.
+   * @param module module number
+   * @param moduleType module type
+   * @return module
+   */
+  static std::shared_ptr<PneumaticsBase> GetForType(
+      int busId, int module, PneumaticsModuleType moduleType);
+
+  /**
+   * For internal use to get the default for a specific type.
+   *
+   * @param moduleType module type
+   * @return module default
+   */
+  static int GetDefaultForType(PneumaticsModuleType moduleType);
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/pneumatic/PneumaticsControlModule.hpp

@@ -0,0 +1,217 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+#include <wpi/DenseMap.h>
+#include <wpi/mutex.h>
+
+#include "PneumaticsBase.h"
+
+namespace frc {
+/** Module class for controlling a Cross The Road Electronics Pneumatics Control
+ * Module. */
+class PneumaticsControlModule : public PneumaticsBase {
+ public:
+  /**
+   * Constructs a PneumaticsControlModule with the default ID (0).
+   *
+   * @param busId The bus ID.
+   */
+  explicit PneumaticsControlModule(int busId);
+
+  /**
+   * Constructs a PneumaticsControlModule.
+   *
+   * @param busId The bus ID.
+   * @param module module number to construct
+   */
+  PneumaticsControlModule(int busId, int module);
+
+  ~PneumaticsControlModule() override = default;
+
+  bool GetCompressor() const override;
+
+  /**
+   * Disables the compressor. The compressor will not turn on until
+   * EnableCompressorDigital() is called.
+   */
+  void DisableCompressor() override;
+
+  void EnableCompressorDigital() override;
+
+  /**
+   * Enables the compressor in digital mode. Analog mode is unsupported by the
+   * CTRE PCM.
+   *
+   * @param minPressure Unsupported.
+   * @param maxPressure Unsupported.
+   * @see EnableCompressorDigital()
+   */
+  void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
+
+  /**
+   * Enables the compressor in digital mode. Hybrid mode is unsupported by the
+   * CTRE PCM.
+   *
+   * @param minPressure Unsupported.
+   * @param maxPressure Unsupported.
+   * @see EnableCompressorDigital()
+   */
+  void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
+
+  CompressorConfigType GetCompressorConfigType() const override;
+
+  bool GetPressureSwitch() const override;
+
+  units::ampere_t GetCompressorCurrent() const override;
+
+  /**
+   * Return whether the compressor current is currently too high.
+   *
+   * @return True if the compressor current is too high, otherwise false.
+   * @see GetCompressorCurrentTooHighStickyFault()
+   */
+  bool GetCompressorCurrentTooHighFault() const;
+
+  /**
+   * Returns whether the compressor current has been too high since sticky
+   * faults were last cleared. This fault is persistent and can be cleared by
+   * ClearAllStickyFaults()
+   *
+   * @return True if the compressor current has been too high since sticky
+   * faults were last cleared.
+   * @see GetCompressorCurrentTooHighFault()
+   */
+  bool GetCompressorCurrentTooHighStickyFault() const;
+
+  /**
+   * Returns whether the compressor is currently shorted.
+   *
+   * @return True if the compressor is currently shorted, otherwise false.
+   * @see GetCompressorShortedStickyFault()
+   */
+  bool GetCompressorShortedFault() const;
+
+  /**
+   * Returns whether the compressor has been shorted since sticky faults were
+   * last cleared. This fault is persistent and can be cleared by
+   * ClearAllStickyFaults()
+   *
+   * @return True if the compressor has been shorted since sticky faults were
+   * last cleared, otherwise false.
+   * @see GetCompressorShortedFault()
+   */
+  bool GetCompressorShortedStickyFault() const;
+
+  /**
+   * Returns whether the compressor is currently disconnected.
+   *
+   * @return True if compressor is currently disconnected, otherwise false.
+   * @see GetCompressorNotConnectedStickyFault()
+   */
+  bool GetCompressorNotConnectedFault() const;
+
+  /**
+   * Returns whether the compressor has been disconnected since sticky faults
+   * were last cleared. This fault is persistent and can be cleared by
+   * ClearAllStickyFaults()
+   *
+   * @return True if the compressor has been disconnected since sticky faults
+   * were last cleared, otherwise false.
+   * @see GetCompressorNotConnectedFault()
+   */
+  bool GetCompressorNotConnectedStickyFault() const;
+
+  /**
+   * Returns whether the solenoid is currently reporting a voltage fault.
+   *
+   * @return True if solenoid is reporting a fault, otherwise false.
+   * @see GetSolenoidVoltageStickyFault()
+   */
+  bool GetSolenoidVoltageFault() const;
+
+  /**
+   * Returns whether the solenoid has reported a voltage fault since sticky
+   * faults were last cleared. This fault is persistent and can be cleared by
+   * ClearAllStickyFaults()
+   *
+   * @return True if solenoid is reporting a fault, otherwise false.
+   * @see GetSolenoidVoltageFault()
+   */
+  bool GetSolenoidVoltageStickyFault() const;
+
+  /** Clears all sticky faults on this device. */
+  void ClearAllStickyFaults();
+
+  void SetSolenoids(int mask, int values) override;
+
+  int GetSolenoids() const override;
+
+  int GetModuleNumber() const override;
+
+  int GetSolenoidDisabledList() const override;
+
+  void FireOneShot(int index) override;
+
+  void SetOneShotDuration(int index, units::second_t duration) override;
+
+  bool CheckSolenoidChannel(int channel) const override;
+
+  int CheckAndReserveSolenoids(int mask) override;
+
+  void UnreserveSolenoids(int mask) override;
+
+  bool ReserveCompressor() override;
+
+  void UnreserveCompressor() override;
+
+  /**
+   * Unsupported by the CTRE PCM.
+   *
+   * @param channel Unsupported.
+   * @return 0
+   */
+  units::volt_t GetAnalogVoltage(int channel) const override;
+
+  /**
+   * Unsupported by the CTRE PCM.
+   *
+   * @param channel Unsupported.
+   * @return 0
+   */
+  units::pounds_per_square_inch_t GetPressure(int channel) const override;
+
+  Solenoid MakeSolenoid(int channel) override;
+  DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
+                                    int reverseChannel) override;
+  Compressor MakeCompressor() override;
+
+  void ReportUsage(std::string_view device, std::string_view data) override;
+
+ private:
+  class DataStore;
+  friend class DataStore;
+  friend class PneumaticsBase;
+  PneumaticsControlModule(int busId, HAL_CTREPCMHandle handle, int module);
+
+  static std::shared_ptr<PneumaticsBase> GetForModule(int busId, int module);
+
+  std::shared_ptr<DataStore> m_dataStore;
+  HAL_CTREPCMHandle m_handle;
+  int m_module;
+
+  static wpi::mutex m_handleLock;
+  static std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<DataStore>>[]>
+      m_handleMaps;
+  static std::weak_ptr<DataStore>& GetDataStore(int busId, int module);
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/pneumatic/PneumaticsModuleType.hpp

@@ -0,0 +1,17 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace frc {
+/**
+ * Pneumatics module type.
+ */
+enum class PneumaticsModuleType {
+  /// CTRE PCM.
+  CTREPCM,
+  /// REV PH.
+  REVPH
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/pneumatic/Solenoid.hpp

@@ -0,0 +1,122 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+#include <units/time.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/PneumaticsBase.h"
+#include "frc/PneumaticsModuleType.h"
+
+namespace frc {
+
+/**
+ * Solenoid class for running high voltage Digital Output on a pneumatics
+ * module.
+ *
+ * The Solenoid class is typically used for pneumatics solenoids, but could be
+ * used for any device within the current spec of the module.
+ */
+class Solenoid : public wpi::Sendable, public wpi::SendableHelper<Solenoid> {
+ public:
+  /**
+   * Constructs a solenoid for a specified module and type.
+   *
+   * @param busId The bus ID.
+   * @param module The module ID to use.
+   * @param moduleType The module type to use.
+   * @param channel The channel the solenoid is on.
+   */
+  Solenoid(int busId, int module, PneumaticsModuleType moduleType, int channel);
+
+  /**
+   * Constructs a solenoid for a default module and specified type.
+   *
+   * @param busId The bus ID.
+   * @param moduleType The module type to use.
+   * @param channel The channel the solenoid is on.
+   */
+  Solenoid(int busId, PneumaticsModuleType moduleType, int channel);
+
+  ~Solenoid() override;
+
+  Solenoid(Solenoid&&) = default;
+  Solenoid& operator=(Solenoid&&) = default;
+
+  /**
+   * Set the value of a solenoid.
+   *
+   * @param on Turn the solenoid output off or on.
+   */
+  void Set(bool on);
+
+  /**
+   * Read the current value of the solenoid.
+   *
+   * @return The current value of the solenoid.
+   */
+  bool Get() const;
+
+  /**
+   * Toggle the value of the solenoid.
+   *
+   * If the solenoid is set to on, it'll be turned off. If the solenoid is set
+   * to off, it'll be turned on.
+   */
+  void Toggle();
+
+  /**
+   * Get the channel this solenoid is connected to.
+   */
+  int GetChannel() const;
+
+  /**
+   * Check if solenoid is Disabled.
+   *
+   * If a solenoid is shorted, it is added to the DisabledList and
+   * disabled until power cycle, or until faults are cleared.
+   *
+   * @see ClearAllPCMStickyFaults()
+   *
+   * @return If solenoid is disabled due to short.
+   */
+  bool IsDisabled() const;
+
+  /**
+   * Set the pulse duration in the pneumatics module. This is used in
+   * conjunction with the startPulse method to allow the pneumatics module to
+   * control the timing of a pulse.
+   *
+   * On the PCM, the timing can be controlled in 0.01 second increments, with a
+   * maximum of 2.55 seconds. On the PH, the timing can be controlled in 0.001
+   * second increments, with a maximum of 65.534 seconds.
+   *
+   * @param duration The duration of the pulse.
+   *
+   * @see startPulse()
+   */
+  void SetPulseDuration(units::second_t duration);
+
+  /**
+   * %Trigger the pneumatics module to generate a pulse of the duration set in
+   * setPulseDuration.
+   *
+   * @see setPulseDuration()
+   */
+  void StartPulse();
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  std::shared_ptr<PneumaticsBase> m_module;
+  int m_mask;
+  int m_channel;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/power/PowerDistribution.hpp

@@ -0,0 +1,353 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <vector>
+
+#include <hal/PowerDistribution.h>
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+
+/**
+ * Class for getting voltage, current, temperature, power and energy from the
+ * CTRE Power Distribution Panel (PDP) or REV Power Distribution Hub (PDH).
+ */
+class PowerDistribution : public wpi::Sendable,
+                          public wpi::SendableHelper<PowerDistribution> {
+ public:
+  /// Default module number.
+  static constexpr int kDefaultModule = -1;
+
+  /**
+   * Power distribution module type.
+   */
+  enum class ModuleType {
+    /// CTRE (Cross The Road Electronics) CTRE Power Distribution Panel (PDP).
+    kCTRE = 1,
+    /// REV Power Distribution Hub (PDH).
+    kRev = 2
+  };
+
+  /**
+   * Constructs a PowerDistribution object.
+   *
+   * Detects the connected PDP/PDH using the default CAN ID (0 for CTRE and 1
+   * for REV).
+   *
+   * @param busId The bus ID.
+   */
+  explicit PowerDistribution(int busId);
+
+  /**
+   * Constructs a PowerDistribution object.
+   *
+   * @param busId The bus ID.
+   * @param module The CAN ID of the PDP/PDH
+   * @param moduleType The type of module
+   */
+  PowerDistribution(int busId, int module, ModuleType moduleType);
+
+  PowerDistribution(PowerDistribution&&) = default;
+  PowerDistribution& operator=(PowerDistribution&&) = default;
+
+  ~PowerDistribution() override = default;
+
+  /**
+   * Gets the number of channels for this power distribution object.
+   *
+   * @return Number of output channels (16 for PDP, 24 for PDH).
+   */
+  int GetNumChannels() const;
+
+  /**
+   * Query the input voltage of the PDP/PDH.
+   *
+   * @return The input voltage in volts
+   */
+  double GetVoltage() const;
+
+  /**
+   * Query the temperature of the PDP.
+   *
+   * Not supported on the Rev PDH and returns 0.
+   *
+   *
+   * @return The temperature in degrees Celsius
+   */
+  double GetTemperature() const;
+
+  /**
+   * Query the current of a single channel of the PDP/PDH.
+   *
+   * @param channel the channel to query (0-15 for PDP, 0-23 for PDH)
+   * @return The current of the channel in Amperes
+   */
+  double GetCurrent(int channel) const;
+
+  /**
+   * Query all currents of the PDP.
+   *
+   * @return The current of each channel in Amperes
+   */
+  std::vector<double> GetAllCurrents() const;
+
+  /**
+   * Query the total current of all monitored PDP/PDH channels.
+   *
+   * @return The total current drawn from all channels in Amperes
+   */
+  double GetTotalCurrent() const;
+
+  /**
+   * Query the total power drawn from all monitored PDP channels.
+   *
+   * Not supported on the Rev PDH and returns 0.
+   *
+   * @return The total power drawn in Watts
+   */
+  double GetTotalPower() const;
+
+  /**
+   * Query the total energy drawn from the monitored PDP channels.
+   *
+   * Not supported on the Rev PDH and returns 0.
+   *
+   * @return The total energy drawn in Joules
+   */
+  double GetTotalEnergy() const;
+
+  /**
+   * Reset the total energy drawn from the PDP.
+   *
+   * Not supported on the Rev PDH and does nothing.
+   *
+   * @see PowerDistribution#GetTotalEnergy
+   */
+  void ResetTotalEnergy();
+
+  /**
+   * Remove all of the fault flags on the PDP/PDH.
+   */
+  void ClearStickyFaults();
+
+  /**
+   * Gets module number (CAN ID).
+   */
+  int GetModule() const;
+
+  /**
+   * Gets module type.
+   */
+  ModuleType GetType() const;
+
+  /**
+   * Gets whether the PDH switchable channel is turned on or off. Returns false
+   * with the CTRE PDP.
+   *
+   * @return The output state of the PDH switchable channel
+   */
+  bool GetSwitchableChannel() const;
+
+  /**
+   * Sets the PDH switchable channel on or off. Does nothing with the CTRE PDP.
+   *
+   * @param enabled Whether to turn the PDH switchable channel on or off
+   */
+  void SetSwitchableChannel(bool enabled);
+
+  /** Version and device data received from a PowerDistribution device */
+  struct Version {
+    /** Firmware major version number. */
+    uint32_t FirmwareMajor;
+    /** Firmware minor version number. */
+    uint32_t FirmwareMinor;
+    /** Firmware fix version number. */
+    uint32_t FirmwareFix;
+    /** Hardware minor version number. */
+    uint32_t HardwareMinor;
+    /** Hardware major version number. */
+    uint32_t HardwareMajor;
+    /** Unique ID. */
+    uint32_t UniqueId;
+  };
+
+  Version GetVersion() const;
+
+  /**
+   * Faults for a PowerDistribution device. These faults are only active while
+   * the condition is active.
+   */
+  struct Faults {
+    /** Breaker fault on channel 0. */
+    uint32_t Channel0BreakerFault : 1;
+    /** Breaker fault on channel 1. */
+    uint32_t Channel1BreakerFault : 1;
+    /** Breaker fault on channel 2. */
+    uint32_t Channel2BreakerFault : 1;
+    /** Breaker fault on channel 3. */
+    uint32_t Channel3BreakerFault : 1;
+    /** Breaker fault on channel 4. */
+    uint32_t Channel4BreakerFault : 1;
+    /** Breaker fault on channel 5. */
+    uint32_t Channel5BreakerFault : 1;
+    /** Breaker fault on channel 6. */
+    uint32_t Channel6BreakerFault : 1;
+    /** Breaker fault on channel 7. */
+    uint32_t Channel7BreakerFault : 1;
+    /** Breaker fault on channel 8. */
+    uint32_t Channel8BreakerFault : 1;
+    /** Breaker fault on channel 9. */
+    uint32_t Channel9BreakerFault : 1;
+    /** Breaker fault on channel 10. */
+    uint32_t Channel10BreakerFault : 1;
+    /** Breaker fault on channel 12. */
+    uint32_t Channel11BreakerFault : 1;
+    /** Breaker fault on channel 13. */
+    uint32_t Channel12BreakerFault : 1;
+    /** Breaker fault on channel 14. */
+    uint32_t Channel13BreakerFault : 1;
+    /** Breaker fault on channel 15. */
+    uint32_t Channel14BreakerFault : 1;
+    /** Breaker fault on channel 16. */
+    uint32_t Channel15BreakerFault : 1;
+    /** Breaker fault on channel 17. */
+    uint32_t Channel16BreakerFault : 1;
+    /** Breaker fault on channel 18. */
+    uint32_t Channel17BreakerFault : 1;
+    /** Breaker fault on channel 19. */
+    uint32_t Channel18BreakerFault : 1;
+    /** Breaker fault on channel 20. */
+    uint32_t Channel19BreakerFault : 1;
+    /** Breaker fault on channel 21. */
+    uint32_t Channel20BreakerFault : 1;
+    /** Breaker fault on channel 22. */
+    uint32_t Channel21BreakerFault : 1;
+    /** Breaker fault on channel 23. */
+    uint32_t Channel22BreakerFault : 1;
+    /** Breaker fault on channel 24. */
+    uint32_t Channel23BreakerFault : 1;
+    /** The input voltage is below the minimum voltage. */
+    uint32_t Brownout : 1;
+    /** A warning was raised by the device's CAN controller. */
+    uint32_t CanWarning : 1;
+    /** The hardware on the device has malfunctioned. */
+    uint32_t HardwareFault : 1;
+
+    /**
+     * Gets whether there is a breaker fault at a specified channel.
+     * @param channel Channel to check for faults.
+     * @return If there is a breaker fault.
+     * @throws A ChannelIndexOutOfRange error if the given int is outside of the
+     * range supported by the hardware.
+     */
+    bool GetBreakerFault(int channel) const;
+  };
+
+  /**
+   * Returns the power distribution faults.
+   *
+   * On a CTRE PDP, this will return an object with no faults active.
+   *
+   * @return The power distribution faults.
+   */
+  Faults GetFaults() const;
+
+  /**
+   * Sticky faults for a PowerDistribution device. These faults will remain
+   * active until they are reset by the user.
+   */
+  struct StickyFaults {
+    /** Breaker fault on channel 0. */
+    uint32_t Channel0BreakerFault : 1;
+    /** Breaker fault on channel 1. */
+    uint32_t Channel1BreakerFault : 1;
+    /** Breaker fault on channel 2. */
+    uint32_t Channel2BreakerFault : 1;
+    /** Breaker fault on channel 3. */
+    uint32_t Channel3BreakerFault : 1;
+    /** Breaker fault on channel 4. */
+    uint32_t Channel4BreakerFault : 1;
+    /** Breaker fault on channel 5. */
+    uint32_t Channel5BreakerFault : 1;
+    /** Breaker fault on channel 6. */
+    uint32_t Channel6BreakerFault : 1;
+    /** Breaker fault on channel 7. */
+    uint32_t Channel7BreakerFault : 1;
+    /** Breaker fault on channel 8. */
+    uint32_t Channel8BreakerFault : 1;
+    /** Breaker fault on channel 9. */
+    uint32_t Channel9BreakerFault : 1;
+    /** Breaker fault on channel 10. */
+    uint32_t Channel10BreakerFault : 1;
+    /** Breaker fault on channel 12. */
+    uint32_t Channel11BreakerFault : 1;
+    /** Breaker fault on channel 13. */
+    uint32_t Channel12BreakerFault : 1;
+    /** Breaker fault on channel 14. */
+    uint32_t Channel13BreakerFault : 1;
+    /** Breaker fault on channel 15. */
+    uint32_t Channel14BreakerFault : 1;
+    /** Breaker fault on channel 16. */
+    uint32_t Channel15BreakerFault : 1;
+    /** Breaker fault on channel 17. */
+    uint32_t Channel16BreakerFault : 1;
+    /** Breaker fault on channel 18. */
+    uint32_t Channel17BreakerFault : 1;
+    /** Breaker fault on channel 19. */
+    uint32_t Channel18BreakerFault : 1;
+    /** Breaker fault on channel 20. */
+    uint32_t Channel19BreakerFault : 1;
+    /** Breaker fault on channel 21. */
+    uint32_t Channel20BreakerFault : 1;
+    /** Breaker fault on channel 22. */
+    uint32_t Channel21BreakerFault : 1;
+    /** Breaker fault on channel 23. */
+    uint32_t Channel22BreakerFault : 1;
+    /** Breaker fault on channel 24. */
+    uint32_t Channel23BreakerFault : 1;
+    /** The input voltage is below the minimum voltage. */
+    uint32_t Brownout : 1;
+    /** A warning was raised by the device's CAN controller. */
+    uint32_t CanWarning : 1;
+    /** The device's CAN controller experienced a "Bus Off" event. */
+    uint32_t CanBusOff : 1;
+    /** The hardware on the device has malfunctioned. */
+    uint32_t HardwareFault : 1;
+    /** The firmware on the device has malfunctioned. */
+    uint32_t FirmwareFault : 1;
+    /** The device has rebooted. */
+    uint32_t HasReset : 1;
+
+    /**
+     * Gets whether there is a sticky breaker fault at the specified channel.
+     * @param channel Index to check for sticky faults.
+     * @return True if there is a sticky breaker fault at the channel, otherwise
+     * false.
+     * @throws A ChannelIndexOutOfRange error if the provided channel is outside
+     * of the range supported by the hardware.
+     */
+    bool GetBreakerFault(int channel) const;
+  };
+
+  /**
+   * Returns the power distribution sticky faults.
+   *
+   * On a CTRE PDP, this will return an object with no faults active.
+   *
+   * @return The power distribution sticky faults.
+   */
+  StickyFaults GetStickyFaults() const;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  hal::Handle<HAL_PowerDistributionHandle, HAL_CleanPowerDistribution> m_handle;
+  int m_module;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/range/SharpIR.hpp

@@ -0,0 +1,100 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/length.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/AnalogInput.h"
+
+namespace frc {
+
+class SharpIR : public wpi::Sendable, public wpi::SendableHelper<SharpIR> {
+ public:
+  /**
+   * Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring
+   * distances from 20cm to 150cm.
+   *
+   * @param channel Analog input channel the sensor is connected to
+   *
+   * @return sensor object
+   */
+  static SharpIR GP2Y0A02YK0F(int channel);
+
+  /**
+   * Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring
+   * distances from 10cm to 80cm.
+   *
+   * @param channel Analog input channel the sensor is connected to
+   *
+   * @return sensor object
+   */
+  static SharpIR GP2Y0A21YK0F(int channel);
+
+  /**
+   * Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring
+   * distances from 4cm to 30cm.
+   *
+   * @param channel Analog input channel the sensor is connected to
+   *
+   * @return sensor object
+   */
+  static SharpIR GP2Y0A41SK0F(int channel);
+
+  /**
+   * Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring
+   * distances from 2cm to 15cm.
+   *
+   * @param channel Analog input channel the sensor is connected to
+   *
+   * @return sensor object
+   */
+  static SharpIR GP2Y0A51SK0F(int channel);
+
+  /**
+   * Manually construct a SharpIR object. The distance is computed using this
+   * formula: A*v ^ B. Prefer to use one of the static factories to create this
+   * device instead.
+   *
+   * @param channel Analog input channel the sensor is connected to
+   * @param a Constant A
+   * @param b Constant B
+   * @param min Minimum distance to report
+   * @param max Maximum distance to report
+   */
+  SharpIR(int channel, double a, double b, units::meter_t min,
+          units::meter_t max);
+
+  /**
+   * Get the analog input channel number.
+   *
+   * @return analog input channel
+   */
+  int GetChannel() const;
+
+  /**
+   * Get the range from the distance sensor.
+   *
+   * @return range of the target returned by the sensor
+   */
+  units::meter_t GetRange() const;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  AnalogInput m_sensor;
+
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simRange;
+
+  double m_A;
+  double m_B;
+  units::meter_t m_min;
+  units::meter_t m_max;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/rotation/AnalogEncoder.hpp

@@ -0,0 +1,150 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/SimDevice.h>
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+class AnalogInput;
+
+/**
+ * Class for supporting continuous analog encoders, such as the US Digital MA3.
+ */
+class AnalogEncoder : public wpi::Sendable,
+                      public wpi::SendableHelper<AnalogEncoder> {
+ public:
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param channel the analog input channel to attach to
+   */
+  explicit AnalogEncoder(int channel);
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param analogInput the analog input to attach to
+   */
+  explicit AnalogEncoder(AnalogInput& analogInput);
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param analogInput the analog input to attach to
+   */
+  explicit AnalogEncoder(AnalogInput* analogInput);
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param analogInput the analog input to attach to
+   */
+  explicit AnalogEncoder(std::shared_ptr<AnalogInput> analogInput);
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
+   *
+   * @param channel the analog input channel to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  AnalogEncoder(int channel, double fullRange, double expectedZero);
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * @param analogInput the analog input to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  AnalogEncoder(AnalogInput& analogInput, double fullRange,
+                double expectedZero);
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * @param analogInput the analog input to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  AnalogEncoder(AnalogInput* analogInput, double fullRange,
+                double expectedZero);
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * @param analogInput the analog input to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  AnalogEncoder(std::shared_ptr<AnalogInput> analogInput, double fullRange,
+                double expectedZero);
+
+  ~AnalogEncoder() override;
+
+  AnalogEncoder(AnalogEncoder&&) = default;
+  AnalogEncoder& operator=(AnalogEncoder&&) = default;
+
+  /**
+   * Get the encoder value.
+   *
+   * @return the encoder value scaled by the full range input
+   */
+  double Get() const;
+
+  /**
+   * Set the encoder voltage percentage range. Analog sensors are not always
+   * fully stable at the end of their travel ranges. Shrinking this range down
+   * can help mitigate issues with that.
+   *
+   * @param min minimum voltage percentage (0-1 range)
+   * @param max maximum voltage percentage (0-1 range)
+   */
+  void SetVoltagePercentageRange(double min, double max);
+
+  /**
+   * Set if this encoder is inverted.
+   *
+   * @param inverted true to invert the encoder, false otherwise
+   */
+  void SetInverted(bool inverted);
+
+  /**
+   * Get the channel number.
+   *
+   * @return The channel number.
+   */
+  int GetChannel() const;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  void Init(double fullRange, double expectedZero);
+  double MapSensorRange(double pos) const;
+
+  std::shared_ptr<AnalogInput> m_analogInput;
+  double m_fullRange;
+  double m_expectedZero;
+  double m_sensorMin{0.0};
+  double m_sensorMax{1.0};
+  bool m_isInverted{false};
+
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simPosition;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/rotation/AnalogPotentiometer.hpp

@@ -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.
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/AnalogInput.h"
+
+namespace frc {
+
+/**
+ * Class for reading analog potentiometers. Analog potentiometers read in an
+ * analog voltage that corresponds to a position. The position is in whichever
+ * units you choose, by way of the scaling and offset constants passed to the
+ * constructor.
+ */
+class AnalogPotentiometer : public wpi::Sendable,
+                            public wpi::SendableHelper<AnalogPotentiometer> {
+ public:
+  /**
+   * Construct an Analog Potentiometer object from a channel number.
+   *
+   * Use the fullRange and offset values so that the output produces meaningful
+   * values. I.E: you have a 270 degree potentiometer and you want the output to
+   * be degrees with the halfway point as 0 degrees. The fullRange value is
+   * 270.0 degrees and the offset is -135.0 since the halfway point after
+   * scaling is 135 degrees.
+   *
+   * This will calculate the result from the fullRange times the fraction of the
+   * supply voltage, plus the offset.
+   *
+   * @param channel   The Analog Input channel number on the roboRIO the
+   *                  potentiometer is plugged into. 0-3 are on-board and 4-7
+   *                  are on the MXP port.
+   * @param fullRange The value (in desired units) representing the full
+   *                  0-3.3V range of the input.
+   * @param offset    The value (in desired units) representing the
+   *                  angular output at 0V.
+   */
+  explicit AnalogPotentiometer(int channel, double fullRange = 1.0,
+                               double offset = 0.0);
+
+  /**
+   * Construct an Analog Potentiometer object from an existing Analog Input
+   * pointer.
+   *
+   * Use the fullRange and offset values so that the output produces meaningful
+   * values. I.E: you have a 270 degree potentiometer and you want the output to
+   * be degrees with the halfway point as 0 degrees. The fullRange value is
+   * 270.0 degrees and the offset is -135.0 since the halfway point after
+   * scaling is 135 degrees.
+   *
+   * This will calculate the result from the fullRange times the fraction of the
+   * supply voltage, plus the offset.
+   *
+   * @param input     The existing Analog Input pointer
+   * @param fullRange The value (in desired units) representing the full
+   *                  0-3.3V range of the input.
+   * @param offset    The value (in desired units) representing the
+   *                  angular output at 0V.
+   */
+  explicit AnalogPotentiometer(AnalogInput* input, double fullRange = 1.0,
+                               double offset = 0.0);
+
+  /**
+   * Construct an Analog Potentiometer object from an existing Analog Input
+   * pointer.
+   *
+   * Use the fullRange and offset values so that the output produces meaningful
+   * values. I.E: you have a 270 degree potentiometer and you want the output to
+   * be degrees with the halfway point as 0 degrees. The fullRange value is
+   * 270.0 degrees and the offset is -135.0 since the halfway point after
+   * scaling is 135 degrees.
+   *
+   * This will calculate the result from the fullRange times the fraction of the
+   * supply voltage, plus the offset.
+   *
+   * @param input     The existing Analog Input pointer
+   * @param fullRange The value (in desired units) representing the full
+   *                  0-3.3V range of the input.
+   * @param offset    The value (in desired units) representing the
+   *                  angular output at 0V.
+   */
+  explicit AnalogPotentiometer(std::shared_ptr<AnalogInput> input,
+                               double fullRange = 1.0, double offset = 0.0);
+
+  ~AnalogPotentiometer() override = default;
+
+  AnalogPotentiometer(AnalogPotentiometer&&) = default;
+  AnalogPotentiometer& operator=(AnalogPotentiometer&&) = default;
+
+  /**
+   * Get the current reading of the potentiometer.
+   *
+   * @return The current position of the potentiometer (in the units used for
+   *         fullRange and offset).
+   */
+  double Get() const;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  std::shared_ptr<AnalogInput> m_analog_input;
+  double m_fullRange, m_offset;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/rotation/DutyCycle.hpp

@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/DutyCycle.h>
+#include <hal/Types.h>
+#include <units/frequency.h>
+#include <units/time.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+/**
+ * Class to read a duty cycle PWM input.
+ *
+ * <p>PWM input signals are specified with a frequency and a ratio of high to
+ * low in that frequency. These can be attached to any SmartIO.
+ *
+ */
+class DutyCycle : public wpi::Sendable, public wpi::SendableHelper<DutyCycle> {
+ public:
+  /**
+   * Constructs a DutyCycle input from a smartio channel.
+   *
+   * @param source The channel to use.
+   */
+  explicit DutyCycle(int source);
+
+  DutyCycle(DutyCycle&&) = default;
+  DutyCycle& operator=(DutyCycle&&) = default;
+
+  /**
+   * Close the DutyCycle and free all resources.
+   */
+  ~DutyCycle() override = default;
+
+  /**
+   * Get the frequency of the duty cycle signal.
+   *
+   * @return frequency
+   */
+  units::hertz_t GetFrequency() const;
+
+  /**
+   * Get the output ratio of the duty cycle signal.
+   *
+   * <p> 0 means always low, 1 means always high.
+   *
+   * @return output ratio between 0 and 1
+   */
+  double GetOutput() const;
+
+  /**
+   * Get the raw high time of the duty cycle signal.
+   *
+   * @return high time of last pulse
+   */
+  units::second_t GetHighTime() const;
+
+  /**
+   * Get the channel of the source.
+   *
+   * @return the source channel
+   */
+  int GetSourceChannel() const;
+
+ protected:
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  void InitDutyCycle();
+  int m_channel;
+  hal::Handle<HAL_DutyCycleHandle, HAL_FreeDutyCycle> m_handle;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/rotation/DutyCycleEncoder.hpp

@@ -0,0 +1,199 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/SimDevice.h>
+#include <hal/Types.h>
+#include <units/frequency.h>
+#include <units/time.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+class DutyCycle;
+
+/**
+ * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with
+ * PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
+ * Encoder.
+ */
+class DutyCycleEncoder : public wpi::Sendable,
+                         public wpi::SendableHelper<DutyCycleEncoder> {
+ public:
+  /**
+   * Construct a new DutyCycleEncoder on a specific channel.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param channel the channel to attach to
+   */
+  explicit DutyCycleEncoder(int channel);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   */
+  explicit DutyCycleEncoder(DutyCycle& dutyCycle);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   */
+  explicit DutyCycleEncoder(DutyCycle* dutyCycle);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * <p>This has a fullRange of 1 and an expectedZero of 0.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   */
+  explicit DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle);
+
+  /**
+   * Construct a new DutyCycleEncoder on a specific channel.
+   *
+   * @param channel the channel to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  DutyCycleEncoder(int channel, double fullRange, double expectedZero);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  DutyCycleEncoder(DutyCycle& dutyCycle, double fullRange, double expectedZero);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  DutyCycleEncoder(DutyCycle* dutyCycle, double fullRange, double expectedZero);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   * @param fullRange the value to report at maximum travel
+   * @param expectedZero the reading where you would expect a 0 from get()
+   */
+  DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle, double fullRange,
+                   double expectedZero);
+
+  ~DutyCycleEncoder() override = default;
+
+  DutyCycleEncoder(DutyCycleEncoder&&) = default;
+  DutyCycleEncoder& operator=(DutyCycleEncoder&&) = default;
+
+  /**
+   * Get the frequency of the duty cycle signal from the encoder.
+   *
+   * @return duty cycle frequency
+   */
+  units::hertz_t GetFrequency() const;
+
+  /**
+   * Get if the sensor is connected
+   *
+   * This uses the duty cycle frequency to determine if the sensor is connected.
+   * By default, a value of 100 Hz is used as the threshold, and this value can
+   * be changed with SetConnectedFrequencyThreshold.
+   *
+   * @return true if the sensor is connected
+   */
+  bool IsConnected() const;
+
+  /**
+   * Change the frequency threshold for detecting connection used by
+   * IsConnected.
+   *
+   * @param frequency the minimum frequency.
+   */
+  void SetConnectedFrequencyThreshold(units::hertz_t frequency);
+
+  /**
+   * Get the encoder value.
+   *
+   * @return the encoder value scaled by the full range input
+   */
+  double Get() const;
+
+  /**
+   * Set the encoder duty cycle range. As the encoder needs to maintain a duty
+   * cycle, the duty cycle cannot go all the way to 0% or all the way to 100%.
+   * For example, an encoder with a 4096 us period might have a minimum duty
+   * cycle of 1 us / 4096 us and a maximum duty cycle of 4095 / 4096 us. Setting
+   * the range will result in an encoder duty cycle less than or equal to the
+   * minimum being output as 0 rotation, the duty cycle greater than or equal to
+   * the maximum being output as 1 rotation, and values in between linearly
+   * scaled from 0 to 1.
+   *
+   * @param min minimum duty cycle (0-1 range)
+   * @param max maximum duty cycle (0-1 range)
+   */
+  void SetDutyCycleRange(double min, double max);
+
+  /**
+   * Sets the assumed frequency of the connected device.
+   *
+   * <p>By default, the DutyCycle engine has to compute the frequency of the
+   * input signal. This can result in both delayed readings and jumpy readings.
+   * To solve this, you can pass the expected frequency of the sensor to this
+   * function. This will use that frequency to compute the DutyCycle percentage,
+   * rather than the computed frequency.
+   *
+   * @param frequency the assumed frequency of the sensor
+   */
+  void SetAssumedFrequency(units::hertz_t frequency);
+
+  /**
+   * Set if this encoder is inverted.
+   *
+   * @param inverted true to invert the encoder, false otherwise
+   */
+  void SetInverted(bool inverted);
+
+  /**
+   * Get the channel of the source.
+   *
+   * @return the source channel
+   */
+  int GetSourceChannel() const;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  void Init(double fullRange, double expectedZero);
+  double MapSensorRange(double pos) const;
+
+  std::shared_ptr<DutyCycle> m_dutyCycle;
+  units::hertz_t m_frequencyThreshold = {100_Hz};
+  double m_fullRange;
+  double m_expectedZero;
+  units::second_t m_period{0_s};
+  double m_sensorMin{0.0};
+  double m_sensorMax{1.0};
+  bool m_isInverted{false};
+
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simPosition;
+  hal::SimBoolean m_simIsConnected;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/hardware/rotation/Encoder.hpp

@@ -0,0 +1,283 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Encoder.h>
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/CounterBase.h"
+
+namespace frc {
+/**
+ * Class to read quad encoders.
+ *
+ * Quadrature encoders are devices that count shaft rotation and can sense
+ * direction. The output of the QuadEncoder class is an integer that can count
+ * either up or down, and can go negative for reverse direction counting. When
+ * creating QuadEncoders, a direction is supplied that changes the sense of the
+ * output to make code more readable if the encoder is mounted such that forward
+ * movement generates negative values. Quadrature encoders have two digital
+ * outputs, an A Channel and a B Channel that are out of phase with each other
+ * to allow the FPGA to do direction sensing.
+ *
+ * All encoders will immediately start counting - Reset() them if you need them
+ * to be zeroed before use.
+ */
+class Encoder : public CounterBase,
+                public wpi::Sendable,
+                public wpi::SendableHelper<Encoder> {
+ public:
+  /**
+   * Encoder constructor.
+   *
+   * Construct a Encoder given a and b channels.
+   *
+   * The counter will start counting immediately.
+   *
+   * @param aChannel         The a channel DIO channel. 0-9 are on-board, 10-25
+   *                         are on the MXP port
+   * @param bChannel         The b channel DIO channel. 0-9 are on-board, 10-25
+   *                         are on the MXP port
+   * @param reverseDirection represents the orientation of the encoder and
+   *                         inverts the output values if necessary so forward
+   *                         represents positive values.
+   * @param encodingType     either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+   *                         decoding. If 4X is selected, then an encoder FPGA
+   *                         object is used and the returned counts will be 4x
+   *                         the encoder spec'd value since all rising and
+   *                         falling edges are counted. If 1X or 2X are selected
+   *                         then a counter object will be used and the returned
+   *                         value will either exactly match the spec'd count or
+   *                         be double (2x) the spec'd count.
+   */
+  Encoder(int aChannel, int bChannel, bool reverseDirection = false,
+          EncodingType encodingType = k4X);
+
+  Encoder(Encoder&&) = default;
+  Encoder& operator=(Encoder&&) = default;
+
+  ~Encoder() override = default;
+
+  // CounterBase interface
+  /**
+   * Gets the current count.
+   *
+   * Returns the current count on the Encoder. This method compensates for the
+   * decoding type.
+   *
+   * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale
+   *         factor.
+   */
+  int Get() const override;
+
+  /**
+   * Reset the Encoder distance to zero.
+   *
+   * Resets the current count to zero on the encoder.
+   */
+  void Reset() override;
+
+  /**
+   * Returns the period of the most recent pulse.
+   *
+   * Returns the period of the most recent Encoder pulse in seconds. This method
+   * compensates for the decoding type.
+   *
+   * Warning: This returns unscaled periods. Use GetRate() for rates that are
+   * scaled using the value from SetDistancePerPulse().
+   *
+   * @return Period in seconds of the most recent pulse.
+   * @deprecated Use getRate() in favor of this method.
+   */
+  [[deprecated("Use GetRate() in favor of this method")]]
+  units::second_t GetPeriod() const override;
+
+  /**
+   * Sets the maximum period for stopped detection.
+   *
+   * Sets the value that represents the maximum period of the Encoder before it
+   * will assume that the attached device is stopped. This timeout allows users
+   * to determine if the wheels or other shaft has stopped rotating.
+   * This method compensates for the decoding type.
+   *
+   * @param maxPeriod The maximum time between rising and falling edges before
+   *                  the FPGA will report the device stopped. This is expressed
+   *                  in seconds.
+   * @deprecated Use SetMinRate() in favor of this method.  This takes unscaled
+   *             periods and SetMinRate() scales using value from
+   *             SetDistancePerPulse().
+   */
+  [[deprecated(
+      "Use SetMinRate() in favor of this method.  This takes unscaled periods "
+      "and SetMinRate() scales using value from SetDistancePerPulse().")]]
+  void SetMaxPeriod(units::second_t maxPeriod) override;
+
+  /**
+   * Determine if the encoder is stopped.
+   *
+   * Using the MaxPeriod value, a boolean is returned that is true if the
+   * encoder is considered stopped and false if it is still moving. A stopped
+   * encoder is one where the most recent pulse width exceeds the MaxPeriod.
+   *
+   * @return True if the encoder is considered stopped.
+   */
+  bool GetStopped() const override;
+
+  /**
+   * The last direction the encoder value changed.
+   *
+   * @return The last direction the encoder value changed.
+   */
+  bool GetDirection() const override;
+
+  /**
+   * Gets the raw value from the encoder.
+   *
+   * The raw value is the actual count unscaled by the 1x, 2x, or 4x scale
+   * factor.
+   *
+   * @return Current raw count from the encoder
+   */
+  int GetRaw() const;
+
+  /**
+   * The encoding scale factor 1x, 2x, or 4x, per the requested encodingType.
+   *
+   * Used to divide raw edge counts down to spec'd counts.
+   */
+  int GetEncodingScale() const;
+
+  /**
+   * Get the distance the robot has driven since the last reset.
+   *
+   * @return The distance driven since the last reset as scaled by the value
+   *         from SetDistancePerPulse().
+   */
+  double GetDistance() const;
+
+  /**
+   * Get the current rate of the encoder.
+   *
+   * Units are distance per second as scaled by the value from
+   * SetDistancePerPulse().
+   *
+   * @return The current rate of the encoder.
+   */
+  double GetRate() const;
+
+  /**
+   * Set the minimum rate of the device before the hardware reports it stopped.
+   *
+   * @param minRate The minimum rate.  The units are in distance per second as
+   *                scaled by the value from SetDistancePerPulse().
+   */
+  void SetMinRate(double minRate);
+
+  /**
+   * Set the distance per pulse for this encoder.
+   *
+   * This sets the multiplier used to determine the distance driven based on the
+   * count value from the encoder.
+   *
+   * Do not include the decoding type in this scale.  The library already
+   * compensates for the decoding type.
+   *
+   * Set this value based on the encoder's rated Pulses per Revolution and
+   * factor in gearing reductions following the encoder shaft.
+   *
+   * This distance can be in any units you like, linear or angular.
+   *
+   * @param distancePerPulse The scale factor that will be used to convert
+   *                         pulses to useful units.
+   */
+  void SetDistancePerPulse(double distancePerPulse);
+
+  /**
+   * Get the distance per pulse for this encoder.
+   *
+   * @return The scale factor that will be used to convert pulses to useful
+   *         units.
+   */
+  double GetDistancePerPulse() const;
+
+  /**
+   * Set the direction sensing for this encoder.
+   *
+   * This sets the direction sensing on the encoder so that it could count in
+   * the correct software direction regardless of the mounting.
+   *
+   * @param reverseDirection true if the encoder direction should be reversed
+   */
+  void SetReverseDirection(bool reverseDirection);
+
+  /**
+   * Set the Samples to Average which specifies the number of samples of the
+   * timer to average when calculating the period.
+   *
+   * Perform averaging to account for mechanical imperfections or as
+   * oversampling to increase resolution.
+   *
+   * @param samplesToAverage The number of samples to average from 1 to 127.
+   */
+  void SetSamplesToAverage(int samplesToAverage);
+
+  /**
+   * Get the Samples to Average which specifies the number of samples of the
+   * timer to average when calculating the period.
+   *
+   * Perform averaging to account for mechanical imperfections or as
+   * oversampling to increase resolution.
+   *
+   * @return The number of samples being averaged (from 1 to 127)
+   */
+  int GetSamplesToAverage() const;
+
+  /**
+   * Indicates this encoder is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  void SetSimDevice(HAL_SimDeviceHandle device);
+
+  int GetFPGAIndex() const;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  /**
+   * Common initialization code for Encoders.
+   *
+   * This code allocates resources for Encoders and is common to all
+   * constructors. The counter will start counting immediately.
+   * @param aChannel         The a channel.
+   * @param bChannel         The b channel.
+   * @param reverseDirection If true, counts down instead of up (this is all
+   *                         relative)
+   * @param encodingType     either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+   *                         decoding. If 4X is selected, then an encoder FPGA
+   *                         object is used and the returned counts will be 4x
+   *                         the encoder spec'd value since all rising and
+   *                         falling edges are counted. If 1X or 2X are selected
+   *                         then a counter object will be used and the returned
+   *                         value will either exactly match the spec'd count or
+   *                         be double (2x) the spec'd count.
+   */
+  void InitEncoder(int aChannel, int bChannel, bool reverseDirection,
+                   EncodingType encodingType);
+
+  /**
+   * The scale needed to convert a raw counter value into a number of encoder
+   * pulses.
+   */
+  double DecodingScaleFactor() const;
+
+  hal::Handle<HAL_EncoderHandle, HAL_FreeEncoder> m_encoder;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/internal/DriverStationModeThread.hpp

@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <atomic>
+#include <thread>
+
+namespace frc::internal {
+/**
+ * For internal use only.
+ */
+class DriverStationModeThread {
+ public:
+  /**
+   * For internal use only.
+   */
+  DriverStationModeThread();
+
+  ~DriverStationModeThread();
+
+  DriverStationModeThread(const DriverStationModeThread& other) = delete;
+  DriverStationModeThread(DriverStationModeThread&& other) = delete;
+  DriverStationModeThread& operator=(const DriverStationModeThread& other) =
+      delete;
+  DriverStationModeThread& operator=(DriverStationModeThread&& other) = delete;
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be
+   * executing for diagnostic purposes only.
+   *
+   * @param entering If true, starting disabled code; if false, leaving disabled
+   * code
+   */
+  void InDisabled(bool entering);
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be
+   * executing for diagnostic purposes only.
+   *
+   * @param entering If true, starting autonomous code; if false, leaving
+   * autonomous code
+   */
+  void InAutonomous(bool entering);
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be
+   * executing for diagnostic purposes only.
+   *
+   * @param entering If true, starting teleop code; if false, leaving teleop
+   * code
+   */
+  void InTeleop(bool entering);
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be
+   * executing for diagnostic purposes only.
+   *
+   * @param entering If true, starting test code; if false, leaving test code
+   */
+  void InTest(bool entering);
+
+ private:
+  std::atomic_bool m_keepAlive{false};
+  std::thread m_thread;
+  void Run();
+  bool m_userInDisabled{false};
+  bool m_userInAutonomous{false};
+  bool m_userInTeleop{false};
+  bool m_userInTest{false};
+};
+}  // namespace frc::internal

wpilibc/src/main/native/include/wpi/opmode/IterativeRobotBase.hpp

@@ -0,0 +1,241 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <units/time.h>
+
+#include "frc/RobotBase.h"
+#include "frc/Watchdog.h"
+
+namespace frc {
+
+/**
+ * IterativeRobotBase implements a specific type of robot program framework,
+ * extending the RobotBase class.
+ *
+ * The IterativeRobotBase class does not implement StartCompetition(), so it
+ * should not be used by teams directly.
+ *
+ * This class provides the following functions which are called by the main
+ * loop, StartCompetition(), at the appropriate times:
+ *
+ * DriverStationConnected() -- provide for initialization the first time the DS
+ * is connected
+ *
+ * Init() functions -- each of the following functions is called once when the
+ * appropriate mode is entered:
+ *
+ * \li DisabledInit() -- called each and every time disabled is entered from
+ *   another mode
+ * \li AutonomousInit() -- called each and every time autonomous is entered from
+ *   another mode
+ * \li TeleopInit() -- called each and every time teleop is entered from another
+ *   mode
+ * \li TestInit() -- called each and every time test is entered from another
+ *   mode
+ *
+ * Periodic() functions -- each of these functions is called on an interval:
+ *
+ * \li RobotPeriodic()
+ * \li DisabledPeriodic()
+ * \li AutonomousPeriodic()
+ * \li TeleopPeriodic()
+ * \li TestPeriodic()
+ *
+ * Exit() functions -- each of the following functions is called once when the
+ * appropriate mode is exited:
+ *
+ * \li DisabledExit() -- called each and every time disabled is exited
+ * \li AutonomousExit() -- called each and every time autonomous is exited
+ * \li TeleopExit() -- called each and every time teleop is exited
+ * \li TestExit() -- called each and every time test is exited
+ */
+class IterativeRobotBase : public RobotBase {
+ public:
+  /**
+   * Code that needs to know the DS state should go here.
+   *
+   * Users should override this method for initialization that needs to occur
+   * after the DS is connected, such as needing the alliance information.
+   */
+  virtual void DriverStationConnected();
+
+  /**
+   * Robot-wide simulation initialization code should go here.
+   *
+   * Users should override this method for default Robot-wide simulation
+   * related initialization which will be called when the robot is first
+   * started. It will be called exactly one time after the robot class
+   * constructor is called only when the robot is in simulation.
+   */
+  virtual void SimulationInit();
+
+  /**
+   * Initialization code for disabled mode should go here.
+   *
+   * Users should override this method for initialization code which will be
+   * called each time
+   * the robot enters disabled mode.
+   */
+  virtual void DisabledInit();
+
+  /**
+   * Initialization code for autonomous mode should go here.
+   *
+   * Users should override this method for initialization code which will be
+   * called each time the robot enters autonomous mode.
+   */
+  virtual void AutonomousInit();
+
+  /**
+   * Initialization code for teleop mode should go here.
+   *
+   * Users should override this method for initialization code which will be
+   * called each time the robot enters teleop mode.
+   */
+  virtual void TeleopInit();
+
+  /**
+   * Initialization code for test mode should go here.
+   *
+   * Users should override this method for initialization code which will be
+   * called each time the robot enters test mode.
+   */
+  virtual void TestInit();
+
+  /**
+   * Periodic code for all modes should go here.
+   *
+   * This function is called each time a new packet is received from the driver
+   * station.
+   */
+  virtual void RobotPeriodic();
+
+  /**
+   * Periodic simulation code should go here.
+   *
+   * This function is called in a simulated robot after user code executes.
+   */
+  virtual void SimulationPeriodic();
+
+  /**
+   * Periodic code for disabled mode should go here.
+   *
+   * Users should override this method for code which will be called each time a
+   * new packet is received from the driver station and the robot is in disabled
+   * mode.
+   */
+  virtual void DisabledPeriodic();
+
+  /**
+   * Periodic code for autonomous mode should go here.
+   *
+   * Users should override this method for code which will be called each time a
+   * new packet is received from the driver station and the robot is in
+   * autonomous mode.
+   */
+  virtual void AutonomousPeriodic();
+
+  /**
+   * Periodic code for teleop mode should go here.
+   *
+   * Users should override this method for code which will be called each time a
+   * new packet is received from the driver station and the robot is in teleop
+   * mode.
+   */
+  virtual void TeleopPeriodic();
+
+  /**
+   * Periodic code for test mode should go here.
+   *
+   * Users should override this method for code which will be called each time a
+   * new packet is received from the driver station and the robot is in test
+   * mode.
+   */
+  virtual void TestPeriodic();
+
+  /**
+   * Exit code for disabled mode should go here.
+   *
+   * Users should override this method for code which will be called each time
+   * the robot exits disabled mode.
+   */
+  virtual void DisabledExit();
+
+  /**
+   * Exit code for autonomous mode should go here.
+   *
+   * Users should override this method for code which will be called each time
+   * the robot exits autonomous mode.
+   */
+  virtual void AutonomousExit();
+
+  /**
+   * Exit code for teleop mode should go here.
+   *
+   * Users should override this method for code which will be called each time
+   * the robot exits teleop mode.
+   */
+  virtual void TeleopExit();
+
+  /**
+   * Exit code for test mode should go here.
+   *
+   * Users should override this method for code which will be called each time
+   * the robot exits test mode.
+   */
+  virtual void TestExit();
+
+  /**
+   * Enables or disables flushing NetworkTables every loop iteration.
+   * By default, this is enabled.
+   *
+   * @param enabled True to enable, false to disable
+   * @deprecated Deprecated without replacement.
+   */
+  [[deprecated("Deprecated without replacement.")]]
+  void SetNetworkTablesFlushEnabled(bool enabled);
+
+  /**
+   * Gets time period between calls to Periodic() functions.
+   */
+  units::second_t GetPeriod() const;
+
+  /**
+   * Prints list of epochs added so far and their times.
+   */
+  void PrintWatchdogEpochs();
+
+  /**
+   * Constructor for IterativeRobotBase.
+   *
+   * @param period Period.
+   */
+  explicit IterativeRobotBase(units::second_t period);
+
+  ~IterativeRobotBase() override = default;
+
+ protected:
+  IterativeRobotBase(IterativeRobotBase&&) = default;
+  IterativeRobotBase& operator=(IterativeRobotBase&&) = default;
+
+  /**
+   * Loop function.
+   */
+  void LoopFunc();
+
+ private:
+  enum class Mode { kNone, kDisabled, kAutonomous, kTeleop, kTest };
+
+  Mode m_lastMode = Mode::kNone;
+  units::second_t m_period;
+  Watchdog m_watchdog;
+  bool m_ntFlushEnabled = true;
+  bool m_calledDsConnected = false;
+
+  void PrintLoopOverrunMessage();
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/opmode/RobotBase.hpp

@@ -0,0 +1,281 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <chrono>
+#include <thread>
+
+#include <hal/DriverStation.h>
+#include <hal/HALBase.h>
+#include <hal/Main.h>
+#include <networktables/NetworkTable.h>
+#include <wpi/RuntimeCheck.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+#include <wpi/string.h>
+
+#include "frc/Errors.h"
+#include "frc/RuntimeType.h"
+
+namespace frc {
+
+int RunHALInitialization();
+
+namespace impl {
+#ifndef __FRC_SYSTEMCORE__
+void ResetMotorSafety();
+#endif
+
+template <class Robot>
+void RunRobot(wpi::mutex& m, Robot** robot) {
+  try {
+    static Robot theRobot;
+    {
+      std::scoped_lock lock{m};
+      *robot = &theRobot;
+    }
+    theRobot.StartCompetition();
+  } catch (const frc::RuntimeError& e) {
+    e.Report();
+    FRC_ReportError(
+        err::Error,
+        "The robot program quit unexpectedly."
+        " This is usually due to a code error.\n"
+        "  The above stacktrace can help determine where the error occurred.\n"
+        "  See https://wpilib.org/stacktrace for more information.\n");
+    throw;
+  } catch (const std::exception& e) {
+    HAL_SendError(1, err::Error, 0, e.what(), "", "", 1);
+    throw;
+  }
+}
+
+}  // namespace impl
+
+template <class Robot>
+int StartRobot() {
+  uint32_t foundMajor;
+  uint32_t foundMinor;
+  uint32_t expectedMajor;
+  uint32_t expectedMinor;
+  WPI_String runtimePath;
+  if (!WPI_IsRuntimeValid(&foundMajor, &foundMinor, &expectedMajor,
+                          &expectedMinor, &runtimePath)) {
+    // We could make this error better, however unlike Java, there is only a
+    // single scenario that could be occuring. The entirety of VS is too out
+    // of date. In most cases the linker should detect this, but not always.
+    fmt::println(
+        "Your copy of Visual Studio is out of date. Please update it.\n");
+    return 1;
+  }
+
+  int halInit = RunHALInitialization();
+  if (halInit != 0) {
+    return halInit;
+  }
+
+  static wpi::mutex m;
+  static wpi::condition_variable cv;
+  static Robot* robot = nullptr;
+  static bool exited = false;
+
+  if (HAL_HasMain()) {
+    std::thread thr([] {
+      try {
+        impl::RunRobot<Robot>(m, &robot);
+      } catch (...) {
+        HAL_ExitMain();
+        {
+          std::scoped_lock lock{m};
+          robot = nullptr;
+          exited = true;
+        }
+        cv.notify_all();
+        throw;
+      }
+
+      HAL_ExitMain();
+      {
+        std::scoped_lock lock{m};
+        robot = nullptr;
+        exited = true;
+      }
+      cv.notify_all();
+    });
+
+    HAL_RunMain();
+
+    // signal loop to exit
+    if (robot) {
+      robot->EndCompetition();
+    }
+
+    // prefer to join, but detach to exit if it doesn't exit in a timely manner
+    using namespace std::chrono_literals;
+    std::unique_lock lock{m};
+    if (cv.wait_for(lock, 1s, [] { return exited; })) {
+      thr.join();
+    } else {
+      thr.detach();
+    }
+  } else {
+    impl::RunRobot<Robot>(m, &robot);
+  }
+
+#ifndef __FRC_SYSTEMCORE__
+  frc::impl::ResetMotorSafety();
+#endif
+  HAL_Shutdown();
+
+  return 0;
+}
+
+/**
+ * Implement a Robot Program framework. The RobotBase class is intended to be
+ * subclassed to create a robot program. The user must implement
+ * StartCompetition() which will be called once and is not expected to exit. The
+ * user must also implement EndCompetition(), which signals to the code in
+ * StartCompetition() that it should exit.
+ *
+ * It is not recommended to subclass this class directly - instead subclass
+ * IterativeRobotBase or TimedRobot.
+ */
+class RobotBase {
+ public:
+  /**
+   * Determine if the Robot is currently enabled.
+   *
+   * @return True if the Robot is currently enabled by the Driver Station.
+   */
+  bool IsEnabled() const;
+
+  /**
+   * Determine if the Robot is currently disabled.
+   *
+   * @return True if the Robot is currently disabled by the Driver Station.
+   */
+  bool IsDisabled() const;
+
+  /**
+   * Determine if the robot is currently in Autonomous mode.
+   *
+   * @return True if the robot is currently operating Autonomously as determined
+   *         by the Driver Station.
+   */
+  bool IsAutonomous() const;
+
+  /**
+   * Determine if the robot is currently in Autonomous mode and enabled.
+   *
+   * @return True if the robot us currently operating Autonomously while enabled
+   * as determined by the Driver Station.
+   */
+  bool IsAutonomousEnabled() const;
+
+  /**
+   * Determine if the robot is currently in Operator Control mode.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode as
+   *         determined by the Driver Station.
+   */
+  bool IsTeleop() const;
+
+  /**
+   * Determine if the robot is current in Operator Control mode and enabled.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode while
+   * enabled as determined by the Driver Station.
+   */
+  bool IsTeleopEnabled() const;
+
+  /**
+   * Determine if the robot is currently in Test mode.
+   *
+   * @return True if the robot is currently running in Test mode as determined
+   * by the Driver Station.
+   */
+  bool IsTest() const;
+
+  /**
+   * Determine if the robot is current in Test mode and enabled.
+   *
+   * @return True if the robot is currently operating in Test mode while
+   * enabled as determined by the Driver Station.
+   */
+  bool IsTestEnabled() const;
+
+  /**
+   * Returns the main thread ID.
+   *
+   * @return The main thread ID.
+   */
+  static std::thread::id GetThreadId();
+
+  /**
+   * Start the main robot code. This function will be called once and should not
+   * exit until signalled by EndCompetition()
+   */
+  virtual void StartCompetition() = 0;
+
+  /** Ends the main loop in StartCompetition(). */
+  virtual void EndCompetition() = 0;
+
+  /**
+   * Get the current runtime type.
+   *
+   * @return Current runtime type.
+   */
+  static RuntimeType GetRuntimeType();
+
+  /**
+   * Get if the robot is real.
+   *
+   * @return If the robot is running in the real world.
+   */
+  static constexpr bool IsReal() {
+#ifdef __FRC_SYSTEMCORE__
+    return true;
+#else
+    return false;
+#endif
+  }
+
+  /**
+   * Get if the robot is a simulation.
+   *
+   * @return If the robot is running in simulation.
+   */
+  static constexpr bool IsSimulation() {
+#ifdef __FRC_SYSTEMCORE__
+    return false;
+#else
+    return true;
+#endif
+  }
+
+  /**
+   * Constructor for a generic robot program.
+   *
+   * User code can be placed in the constructor that runs before the
+   * Autonomous or Operator Control period starts. The constructor will run to
+   * completion before Autonomous is entered.
+   *
+   * This must be used to ensure that the communications code starts. In the
+   * future it would be nice to put this code into it's own task that loads on
+   * boot so ensure that it runs.
+   */
+  RobotBase();
+
+  virtual ~RobotBase() = default;
+
+ protected:
+  RobotBase(RobotBase&&) = default;
+  RobotBase& operator=(RobotBase&&) = default;
+
+  static std::thread::id m_threadId;
+  NT_Listener connListenerHandle;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/opmode/RobotState.hpp

@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace frc {
+
+/**
+ * Robot state utility functions.
+ */
+class RobotState {
+ public:
+  RobotState() = delete;
+
+  /**
+   * Returns true if the robot is disabled.
+   *
+   * @return True if the robot is disabled.
+   */
+  static bool IsDisabled();
+
+  /**
+   * Returns true if the robot is enabled.
+   *
+   * @return True if the robot is enabled.
+   */
+  static bool IsEnabled();
+
+  /**
+   * Returns true if the robot is E-stopped.
+   *
+   * @return True if the robot is E-stopped.
+   */
+  static bool IsEStopped();
+
+  /**
+   * Returns true if the robot is in teleop mode.
+   *
+   * @return True if the robot is in teleop mode.
+   */
+  static bool IsTeleop();
+
+  /**
+   * Returns true if the robot is in autonomous mode.
+   *
+   * @return True if the robot is in autonomous mode.
+   */
+  static bool IsAutonomous();
+
+  /**
+   * Returns true if the robot is in test mode.
+   *
+   * @return True if the robot is in test mode.
+   */
+  static bool IsTest();
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/opmode/TimedRobot.hpp

@@ -0,0 +1,131 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <chrono>
+#include <functional>
+#include <utility>
+#include <vector>
+
+#include <hal/Notifier.h>
+#include <hal/Types.h>
+#include <units/frequency.h>
+#include <units/math.h>
+#include <units/time.h>
+#include <wpi/priority_queue.h>
+
+#include "frc/IterativeRobotBase.h"
+#include "frc/RobotController.h"
+
+namespace frc {
+
+/**
+ * TimedRobot implements the IterativeRobotBase robot program framework.
+ *
+ * The TimedRobot class is intended to be subclassed by a user creating a
+ * robot program.
+ *
+ * Periodic() functions from the base class are called on an interval by a
+ * Notifier instance.
+ */
+class TimedRobot : public IterativeRobotBase {
+ public:
+  /// Default loop period.
+  static constexpr auto kDefaultPeriod = 20_ms;
+
+  /**
+   * Provide an alternate "main loop" via StartCompetition().
+   */
+  void StartCompetition() override;
+
+  /**
+   * Ends the main loop in StartCompetition().
+   */
+  void EndCompetition() override;
+
+  /**
+   * Constructor for TimedRobot.
+   *
+   * @param period The period of the robot loop function.
+   */
+  explicit TimedRobot(units::second_t period = kDefaultPeriod);
+
+  /**
+   * Constructor for TimedRobot.
+   *
+   * @param frequency The frequency of the robot loop function.
+   */
+  explicit TimedRobot(units::hertz_t frequency);
+
+  TimedRobot(TimedRobot&&) = default;
+  TimedRobot& operator=(TimedRobot&&) = default;
+
+  ~TimedRobot() override;
+
+  /**
+   * Return the system clock time in micrseconds for the start of the current
+   * periodic loop. This is in the same time base as Timer.GetFPGATimestamp(),
+   * but is stable through a loop. It is updated at the beginning of every
+   * periodic callback (including the normal periodic loop).
+   *
+   * @return Robot running time in microseconds, as of the start of the current
+   * periodic function.
+   */
+  uint64_t GetLoopStartTime();
+
+  /**
+   * Add a callback to run at a specific period with a starting time offset.
+   *
+   * This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback
+   * run synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback The callback to run.
+   * @param period   The period at which to run the callback.
+   * @param offset   The offset from the common starting time. This is useful
+   *                 for scheduling a callback in a different timeslot relative
+   *                 to TimedRobot.
+   */
+  void AddPeriodic(std::function<void()> callback, units::second_t period,
+                   units::second_t offset = 0_s);
+
+ private:
+  class Callback {
+   public:
+    std::function<void()> func;
+    std::chrono::microseconds period;
+    std::chrono::microseconds expirationTime;
+
+    /**
+     * Construct a callback container.
+     *
+     * @param func      The callback to run.
+     * @param startTime The common starting point for all callback scheduling.
+     * @param period    The period at which to run the callback.
+     * @param offset    The offset from the common starting time.
+     */
+    Callback(std::function<void()> func, std::chrono::microseconds startTime,
+             std::chrono::microseconds period, std::chrono::microseconds offset)
+        : func{std::move(func)},
+          period{period},
+          expirationTime(
+              startTime + offset + period +
+              (std::chrono::microseconds{frc::RobotController::GetFPGATime()} -
+               startTime) /
+                  period * period) {}
+
+    bool operator>(const Callback& rhs) const {
+      return expirationTime > rhs.expirationTime;
+    }
+  };
+
+  hal::Handle<HAL_NotifierHandle, HAL_CleanNotifier> m_notifier;
+  std::chrono::microseconds m_startTime;
+  uint64_t m_loopStartTimeUs = 0;
+
+  wpi::priority_queue<Callback, std::vector<Callback>, std::greater<Callback>>
+      m_callbacks;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/opmode/TimesliceRobot.hpp

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

wpilibc/src/main/native/include/wpi/simulation/ADXL345Sim.hpp

@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+
+namespace frc {
+
+class ADXL345_I2C;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADXL345.
+ */
+class ADXL345Sim {
+ public:
+  /**
+   * Constructs from a ADXL345_I2C object.
+   *
+   * @param accel ADXL345 accel to simulate
+   */
+  explicit ADXL345Sim(const ADXL345_I2C& accel);
+
+  /**
+   * Sets the X acceleration.
+   *
+   * @param accel The X acceleration.
+   */
+  void SetX(double accel);
+
+  /**
+   * Sets the Y acceleration.
+   *
+   * @param accel The Y acceleration.
+   */
+  void SetY(double accel);
+
+  /**
+   * Sets the Z acceleration.
+   *
+   * @param accel The Z acceleration.
+   */
+  void SetZ(double accel);
+
+ private:
+  hal::SimDouble m_simX;
+  hal::SimDouble m_simY;
+  hal::SimDouble m_simZ;
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/AddressableLEDSim.hpp

@@ -0,0 +1,168 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+struct HAL_AddressableLEDData;
+
+namespace frc {
+
+class AddressableLED;
+
+namespace sim {
+
+/**
+ * Class to control a simulated addressable LED.
+ */
+class AddressableLEDSim {
+ public:
+  /**
+   * Constructs an addressable LED for a specific channel.
+   *
+   * @param channel output channel
+   */
+  explicit AddressableLEDSim(int channel);
+
+  /**
+   * Constructs from an AddressableLED object.
+   *
+   * @param addressableLED AddressableLED to simulate
+   */
+  explicit AddressableLEDSim(const AddressableLED& addressableLED);
+
+  /**
+   * 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 CallbackStore object storing this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if initialized.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Change the Initialized value of the LED strip.
+   *
+   * @param initialized the new value
+   */
+  void SetInitialized(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterStartCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  /**
+   * Get the start.
+   *
+   * @return the start
+   */
+  int GetStart() const;
+
+  /**
+   * Change the start.
+   *
+   * @param start the new start
+   */
+  void SetStart(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterLengthCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  /**
+   * Get the length of the LED strip.
+   *
+   * @return the length
+   */
+  int GetLength() const;
+
+  /**
+   * Change the length of the LED strip.
+   *
+   * @param length the new value
+   */
+  void SetLength(int length);
+
+  /**
+   * Get the LED data.
+   *
+   * @param data output parameter to fill with LED data
+   * @return the length of the LED data
+   */
+  int GetData(struct HAL_AddressableLEDData* data) const;
+
+  /**
+   * Change the LED data.
+   *
+   * @param data the new data
+   */
+  void SetData(struct HAL_AddressableLEDData* data);
+
+  /**
+   * Register a callback on the LED data.
+   *
+   * @param callback the callback that will be called whenever the LED data is
+   *                 changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterDataCallback(
+      ConstBufferCallback callback, bool initialNotify);
+
+  /**
+   * Get the global LED data.
+   *
+   * @param start the start of the LED data
+   * @param length the length of the LED data
+   * @param data output parameter to fill with LED data
+   * @return the length of the LED data
+   */
+  static int GetGlobalData(int start, int length,
+                           struct HAL_AddressableLEDData* data);
+
+  /**
+   * Change the global LED data.
+   *
+   * @param start the start of the LED data
+   * @param length the length of the LED data
+   * @param data the new data
+   */
+  static void SetGlobalData(int start, int length,
+                            struct HAL_AddressableLEDData* data);
+
+ private:
+  int m_channel;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/AnalogEncoderSim.hpp

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/angle.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+class AnalogEncoder;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog encoder.
+ */
+class AnalogEncoderSim {
+ public:
+  /**
+   * Constructs from an AnalogEncoder object.
+   *
+   * @param encoder AnalogEncoder to simulate
+   */
+  explicit AnalogEncoderSim(const AnalogEncoder& encoder);
+
+  /**
+   * Set the position.
+   *
+   * @param value The position.
+   */
+  void Set(double value);
+
+  /**
+   * Get the simulated position.
+   */
+  double Get();
+
+ private:
+  hal::SimDouble m_positionSim;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/AnalogInputSim.hpp

@@ -0,0 +1,149 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class AnalogInput;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog input.
+ */
+class AnalogInputSim {
+ public:
+  /**
+   * Constructs from an AnalogInput object.
+   *
+   * @param analogInput AnalogInput to simulate
+   */
+  explicit AnalogInputSim(const AnalogInput& analogInput);
+
+  /**
+   * Constructs from an analog input channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit AnalogInputSim(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if this analog input has been initialized.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Change whether this analog input has been initialized.
+   *
+   * @param initialized the new value
+   */
+  void SetInitialized(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterAverageBitsCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the number of average bits.
+   *
+   * @return the number of average bits
+   */
+  int GetAverageBits() const;
+
+  /**
+   * Change the number of average bits.
+   *
+   * @param averageBits the new value
+   */
+  void SetAverageBits(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterOversampleBitsCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the amount of oversampling bits.
+   *
+   * @return the amount of oversampling bits
+   */
+  int GetOversampleBits() const;
+
+  /**
+   * Change the amount of oversampling bits.
+   *
+   * @param oversampleBits the new value
+   */
+  void SetOversampleBits(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the voltage.
+   *
+   * @return the voltage
+   */
+  double GetVoltage() const;
+
+  /**
+   * Change the voltage.
+   *
+   * @param voltage the new value
+   */
+  void SetVoltage(double voltage);
+
+  /**
+   * Reset all simulation data for this object.
+   */
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/BatterySim.hpp

@@ -0,0 +1,92 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <numeric>
+#include <span>
+
+#include <units/current.h>
+#include <units/impedance.h>
+#include <units/voltage.h>
+
+namespace frc::sim {
+
+/**
+ * A utility class to simulate the robot battery.
+ */
+class BatterySim {
+ public:
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * RoboRioSim::SetVInVoltage(double) to set the simulated battery voltage,
+   * which can then be retrieved with the RobotController::GetBatteryVoltage()
+   * method.
+   *
+   * @param nominalVoltage The nominal battery voltage. Usually 12v.
+   * @param resistance     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.
+   */
+  static units::volt_t Calculate(units::volt_t nominalVoltage,
+                                 units::ohm_t resistance,
+                                 std::span<const units::ampere_t> currents) {
+    return std::max(0_V, nominalVoltage - std::accumulate(currents.begin(),
+                                                          currents.end(), 0_A) *
+                                              resistance);
+  }
+
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * RoboRioSim::SetVInVoltage(double) to set the simulated battery voltage,
+   * which can then be retrieved with the RobotController::GetBatteryVoltage()
+   * method.
+   *
+   * @param nominalVoltage The nominal battery voltage. Usually 12v.
+   * @param resistance     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.
+   */
+  static units::volt_t Calculate(
+      units::volt_t nominalVoltage, units::ohm_t resistance,
+      std::initializer_list<units::ampere_t> currents) {
+    return std::max(0_V, nominalVoltage - std::accumulate(currents.begin(),
+                                                          currents.end(), 0_A) *
+                                              resistance);
+  }
+
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * RoboRioSimSetVInVoltage(double) to set the simulated battery voltage, which
+   * can then be retrieved with the 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.
+   */
+  static units::volt_t Calculate(std::span<const units::ampere_t> currents) {
+    return Calculate(12_V, 0.02_Ohm, currents);
+  }
+
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * RoboRioSimSetVInVoltage(double) to set the simulated battery voltage, which
+   * can then be retrieved with the 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.
+   */
+  static units::volt_t Calculate(
+      std::initializer_list<units::ampere_t> currents) {
+    return Calculate(12_V, 0.02_Ohm, currents);
+  }
+};
+
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/CTREPCMSim.hpp

@@ -0,0 +1,143 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/PneumaticsBase.h"
+#include "frc/simulation/CallbackStore.h"
+#include "frc/simulation/PneumaticsBaseSim.h"
+
+namespace frc::sim {
+
+/**
+ * Class to control a simulated Pneumatic Control Module (PCM).
+ */
+class CTREPCMSim : public PneumaticsBaseSim {
+ public:
+  /**
+   * Constructs with the default PCM module number (CAN ID).
+   */
+  CTREPCMSim();
+
+  /**
+   * Constructs from a PCM module number (CAN ID).
+   *
+   * @param module module number
+   */
+  explicit CTREPCMSim(int module);
+
+  explicit CTREPCMSim(const PneumaticsBase& pneumatics);
+
+  ~CTREPCMSim() override = default;
+
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify) override;
+
+  bool GetInitialized() const override;
+
+  void SetInitialized(bool initialized) override;
+
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterSolenoidOutputCallback(
+      int channel, NotifyCallback callback, bool initialNotify) override;
+
+  bool GetSolenoidOutput(int channel) const override;
+
+  void SetSolenoidOutput(int channel, bool solenoidOutput) override;
+
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterCompressorOnCallback(
+      NotifyCallback callback, bool initialNotify) override;
+
+  bool GetCompressorOn() const override;
+
+  void SetCompressorOn(bool compressorOn) override;
+
+  /**
+   * 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterClosedLoopEnabledCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check whether the closed loop compressor control is active.
+   *
+   * @return true if active
+   */
+  bool GetClosedLoopEnabled() const;
+
+  /**
+   * Turn on/off the closed loop control of the compressor.
+   *
+   * @param closedLoopEnabled whether the control loop is active
+   */
+  void SetClosedLoopEnabled(bool closedLoopEnabled);
+
+  /**
+   * 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterPressureSwitchCallback(
+      NotifyCallback callback, bool initialNotify) override;
+
+  /**
+   * Check the value of the pressure switch.
+   *
+   * @return the pressure switch value
+   */
+  bool GetPressureSwitch() const override;
+
+  /**
+   * Set the value of the pressure switch.
+   *
+   * @param pressureSwitch the new value
+   */
+  void SetPressureSwitch(bool pressureSwitch) override;
+
+  /**
+   * 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterCompressorCurrentCallback(
+      NotifyCallback callback, bool initialNotify) override;
+
+  /**
+   * Read the compressor current.
+   *
+   * @return the current of the compressor connected to this module
+   */
+  double GetCompressorCurrent() const override;
+
+  /**
+   * Set the compressor current.
+   *
+   * @param compressorCurrent the new compressor current
+   */
+  void SetCompressorCurrent(double compressorCurrent) override;
+
+  uint8_t GetAllSolenoidOutputs() const override;
+
+  void SetAllSolenoidOutputs(uint8_t outputs) override;
+
+  void ResetData() override;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/CallbackStore.hpp

@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <string_view>
+
+#include <hal/Value.h>
+
+namespace frc::sim {
+
+using NotifyCallback = std::function<void(std::string_view, const HAL_Value*)>;
+using ConstBufferCallback = std::function<void(
+    std::string_view, const unsigned char* buffer, unsigned int count)>;
+using CancelCallbackFunc = void (*)(int32_t index, int32_t uid);
+using CancelCallbackNoIndexFunc = void (*)(int32_t uid);
+using CancelCallbackChannelFunc = void (*)(int32_t index, int32_t channel,
+                                           int32_t uid);
+
+void CallbackStoreThunk(const char* name, void* param, const HAL_Value* value);
+void ConstBufferCallbackStoreThunk(const char* name, void* param,
+                                   const unsigned char* buffer,
+                                   unsigned int count);
+
+/**
+ * Manages simulation callbacks; each object is associated with a callback.
+ */
+class CallbackStore {
+ public:
+  CallbackStore(int32_t i, NotifyCallback cb, CancelCallbackNoIndexFunc ccf);
+
+  CallbackStore(int32_t i, int32_t u, NotifyCallback cb,
+                CancelCallbackFunc ccf);
+
+  CallbackStore(int32_t i, int32_t c, int32_t u, NotifyCallback cb,
+                CancelCallbackChannelFunc ccf);
+
+  CallbackStore(int32_t i, ConstBufferCallback cb,
+                CancelCallbackNoIndexFunc ccf);
+
+  CallbackStore(int32_t i, int32_t u, ConstBufferCallback cb,
+                CancelCallbackFunc ccf);
+
+  CallbackStore(int32_t i, int32_t c, int32_t u, ConstBufferCallback cb,
+                CancelCallbackChannelFunc ccf);
+
+  CallbackStore(const CallbackStore&) = delete;
+  CallbackStore& operator=(const CallbackStore&) = delete;
+
+  ~CallbackStore();
+
+  void SetUid(int32_t uid);
+
+  friend void CallbackStoreThunk(const char* name, void* param,
+                                 const HAL_Value* value);
+
+  friend void ConstBufferCallbackStoreThunk(const char* name, void* param,
+                                            const unsigned char* buffer,
+                                            unsigned int count);
+
+ private:
+  int32_t index;
+  int32_t channel;
+  int32_t uid;
+
+  NotifyCallback callback;
+  ConstBufferCallback constBufferCallback;
+  union {
+    CancelCallbackFunc ccf;
+    CancelCallbackChannelFunc cccf;
+    CancelCallbackNoIndexFunc ccnif;
+  };
+  enum CancelType { Normal, Channel, NoIndex };
+  CancelType cancelType;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/DCMotorSim.hpp

@@ -0,0 +1,131 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <units/angle.h>
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+#include <units/moment_of_inertia.h>
+#include <units/torque.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated DC motor mechanism.
+ */
+class DCMotorSim : public LinearSystemSim<2, 1, 2> {
+ public:
+  /**
+   * Creates a simulated DC motor mechanism.
+   *
+   * @param plant              The linear system representing the DC motor. This
+   * system can be created with LinearSystemId::DCMotorSystem(). If
+   * LinearSystemId::DCMotorSystem(kV, kA) 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 deviation of the measurement noise.
+   */
+  DCMotorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox,
+             const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
+
+  using LinearSystemSim::SetState;
+
+  /**
+   * Sets the state of the DC motor.
+   *
+   * @param angularPosition The new position
+   * @param angularVelocity The new velocity
+   */
+  void SetState(units::radian_t angularPosition,
+                units::radians_per_second_t angularVelocity);
+
+  /**
+   * Sets the DC motor's angular position.
+   *
+   * @param angularPosition The new position in radians.
+   */
+  void SetAngle(units::radian_t angularPosition);
+
+  /**
+   * Sets the DC motor's angular velocity.
+   *
+   * @param angularVelocity The new velocity in radians per second.
+   */
+  void SetAngularVelocity(units::radians_per_second_t angularVelocity);
+
+  /**
+   * Returns the DC motor position.
+   *
+   * @return The DC motor position.
+   */
+  units::radian_t GetAngularPosition() const;
+
+  /**
+   * Returns the DC motor velocity.
+   *
+   * @return The DC motor velocity.
+   */
+  units::radians_per_second_t GetAngularVelocity() const;
+
+  /**
+   * Returns the DC motor acceleration.
+   *
+   * @return The DC motor acceleration
+   */
+  units::radians_per_second_squared_t GetAngularAcceleration() const;
+
+  /**
+   * Returns the DC motor torque.
+   *
+   * @return The DC motor torque
+   */
+  units::newton_meter_t GetTorque() const;
+
+  /**
+   * Returns the DC motor current draw.
+   *
+   * @return The DC motor current draw.
+   */
+  units::ampere_t GetCurrentDraw() const;
+
+  /**
+   * Gets the input voltage for the DC motor.
+   *
+   * @return The DC motor input voltage.
+   */
+  units::volt_t GetInputVoltage() const;
+
+  /**
+   * Sets the input voltage for the DC motor.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+  /**
+   * Returns the gearbox.
+   */
+  const DCMotor& GetGearbox() const;
+
+  /**
+   * Returns the gearing;
+   */
+  double GetGearing() const;
+
+  /**
+   * Returns the moment of inertia
+   */
+  units::kilogram_square_meter_t GetJ() const;
+
+ private:
+  DCMotor m_gearbox;
+  double m_gearing;
+  units::kilogram_square_meter_t m_j;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/DIOSim.hpp

@@ -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.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class DigitalInput;
+class DigitalOutput;
+
+namespace sim {
+
+/**
+ * Class to control a simulated digital input or output.
+ */
+class DIOSim {
+ public:
+  /**
+   * Constructs from a DigitalInput object.
+   *
+   * @param input DigitalInput to simulate
+   */
+  explicit DIOSim(const DigitalInput& input);
+
+  /**
+   * Constructs from a DigitalOutput object.
+   *
+   * @param output DigitalOutput to simulate
+   */
+  explicit DIOSim(const DigitalOutput& output);
+
+  /**
+   * Constructs from an digital I/O channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit DIOSim(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check whether this DIO has been initialized.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Define whether this DIO has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
+  void SetInitialized(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterValueCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  /**
+   * Read the value of the DIO port.
+   *
+   * @return the DIO value
+   */
+  bool GetValue() const;
+
+  /**
+   * Change the DIO value.
+   *
+   * @param value the new value
+   */
+  void SetValue(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterPulseLengthCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Read the pulse length.
+   *
+   * @return the pulse length of this DIO port
+   */
+  double GetPulseLength() const;
+
+  /**
+   * Change the pulse length of this DIO port.
+   *
+   * @param pulseLength the new pulse length
+   */
+  void SetPulseLength(double 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterIsInputCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check whether this DIO port is currently an Input.
+   *
+   * @return true if Input
+   */
+  bool GetIsInput() const;
+
+  /**
+   * Define whether this DIO port is an Input.
+   *
+   * @param isInput whether this DIO should be an Input
+   */
+  void SetIsInput(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterFilterIndexCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Read the filter index.
+   *
+   * @return the filter index of this DIO port
+   */
+  int GetFilterIndex() const;
+
+  /**
+   * Change the filter index of this DIO port.
+   *
+   * @param filterIndex the new filter index
+   */
+  void SetFilterIndex(int filterIndex);
+
+  /**
+   * Reset all simulation data of this object.
+   */
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp

@@ -0,0 +1,361 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/EigenCore.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/system/LinearSystem.h>
+#include <frc/system/plant/DCMotor.h>
+
+#include <units/length.h>
+#include <units/moment_of_inertia.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace frc::sim {
+
+class DifferentialDrivetrainSim {
+ public:
+  /**
+   * Creates a simulated differential drivetrain.
+   *
+   * @param plant The LinearSystem representing the robot's drivetrain. This
+   *              system can be created with
+   *              LinearSystemId::DrivetrainVelocitySystem() or
+   *              LinearSystemId::IdentifyDrivetrainSystem().
+   * @param trackwidth   The robot's trackwidth.
+   * @param driveMotor   A DCMotor representing the left side of the drivetrain.
+   * @param gearingRatio The gearingRatio ratio of the left side, as output over
+   *                     input. This must be the same ratio as the ratio used to
+   *                     identify or create the plant.
+   * @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 omitted
+   *                           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.
+   */
+  DifferentialDrivetrainSim(
+      LinearSystem<2, 2, 2> plant, units::meter_t trackwidth,
+      DCMotor driveMotor, double gearingRatio, units::meter_t wheelRadius,
+      const std::array<double, 7>& measurementStdDevs = {});
+
+  /**
+   * Creates a simulated differential drivetrain.
+   *
+   * @param driveMotor  A DCMotor representing the left side of the drivetrain.
+   * @param gearing     The gearing on the drive between motor and wheel, as
+   *                    output over input. This must be the same ratio as the
+   *                    ratio used to identify or create the plant.
+   * @param J           The moment of inertia of the drivetrain about its
+   *                    center.
+   * @param mass        The mass of the drivebase.
+   * @param wheelRadius The radius of the wheels on the drivetrain.
+   * @param trackwidth  The robot's trackwidth, or distance between left and
+   *                    right wheels.
+   * @param measurementStdDevs Standard deviations for measurements, in the form
+   *                           [x, y, heading, left velocity, right velocity,
+   *                           left distance, right distance]ᵀ. Can be omitted
+   *                           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.
+   */
+  DifferentialDrivetrainSim(
+      frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J,
+      units::kilogram_t mass, units::meter_t wheelRadius,
+      units::meter_t trackwidth,
+      const std::array<double, 7>& measurementStdDevs = {});
+
+  /**
+   * 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.
+   */
+  Eigen::Vector2d ClampInput(const Eigen::Vector2d& u);
+
+  /**
+   * Sets the applied voltage to the drivetrain. Note that positive voltage must
+   * make that side of the drivetrain travel forward (+X).
+   *
+   * @param leftVoltage  The left voltage.
+   * @param rightVoltage The right voltage.
+   */
+  void SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage);
+
+  /**
+   * Sets the gearing reduction on the drivetrain. This is commonly used for
+   * shifting drivetrains.
+   *
+   * @param newGearing The new gear ratio, as output over input.
+   */
+  void SetGearing(double newGearing);
+
+  /**
+   * Updates the simulation.
+   *
+   * @param dt The time that's passed since the last Update(units::second_t)
+   *           call.
+   */
+  void Update(units::second_t dt);
+
+  /**
+   * Returns the current gearing reduction of the drivetrain, as output over
+   * input.
+   */
+  double GetGearing() const;
+
+  /**
+   * Returns the direction the robot is pointing.
+   *
+   * Note that this angle is counterclockwise-positive, while most gyros are
+   * clockwise positive.
+   */
+  Rotation2d GetHeading() const;
+
+  /**
+   * Returns the current pose.
+   */
+  Pose2d GetPose() const;
+
+  /**
+   * Get the right encoder position in meters.
+   * @return The encoder position.
+   */
+  units::meter_t GetRightPosition() const {
+    return units::meter_t{GetOutput(State::kRightPosition)};
+  }
+
+  /**
+   * Get the right encoder velocity in meters per second.
+   * @return The encoder velocity.
+   */
+  units::meters_per_second_t GetRightVelocity() const {
+    return units::meters_per_second_t{GetOutput(State::kRightVelocity)};
+  }
+
+  /**
+   * Get the left encoder position in meters.
+   * @return The encoder position.
+   */
+  units::meter_t GetLeftPosition() const {
+    return units::meter_t{GetOutput(State::kLeftPosition)};
+  }
+
+  /**
+   * Get the left encoder velocity in meters per second.
+   * @return The encoder velocity.
+   */
+  units::meters_per_second_t GetLeftVelocity() const {
+    return units::meters_per_second_t{GetOutput(State::kLeftVelocity)};
+  }
+
+  /**
+   * Returns the currently drawn current for the right side.
+   */
+  units::ampere_t GetRightCurrentDraw() const;
+
+  /**
+   * Returns the currently drawn current for the left side.
+   */
+  units::ampere_t GetLeftCurrentDraw() const;
+
+  /**
+   * Returns the currently drawn current.
+   */
+  units::ampere_t GetCurrentDraw() const;
+
+  /**
+   * Sets the system state.
+   *
+   * @param state The state.
+   */
+  void SetState(const Vectord<7>& state);
+
+  /**
+   * Sets the system pose.
+   *
+   * @param pose The pose.
+   */
+  void SetPose(const frc::Pose2d& pose);
+
+  /**
+   * The differential drive dynamics function.
+   *
+   * @param x The state.
+   * @param u The input.
+   * @return The state derivative with respect to time.
+   */
+  Vectord<7> Dynamics(const Vectord<7>& x, const Eigen::Vector2d& u);
+
+  class State {
+   public:
+    static constexpr int kX = 0;
+    static constexpr int kY = 1;
+    static constexpr int kHeading = 2;
+    static constexpr int kLeftVelocity = 3;
+    static constexpr int kRightVelocity = 4;
+    static constexpr int kLeftPosition = 5;
+    static constexpr int kRightPosition = 6;
+  };
+
+  /**
+   * 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
+   */
+  class KitbotGearing {
+   public:
+    /// Gear ratio of 12.75:1.
+    static constexpr double k12p75 = 12.75;
+    /// Gear ratio of 10.71:1.
+    static constexpr double k10p71 = 10.71;
+    /// Gear ratio of 8.45:1.
+    static constexpr double k8p45 = 8.45;
+    /// Gear ratio of 7.31:1.
+    static constexpr double k7p31 = 7.31;
+    /// Gear ratio of 5.95:1.
+    static constexpr double k5p95 = 5.95;
+  };
+
+  /**
+   * Represents common motor layouts of the kit drivetrain.
+   */
+  class KitbotMotor {
+   public:
+    /// One CIM motor per drive side.
+    static constexpr frc::DCMotor SingleCIMPerSide = frc::DCMotor::CIM(1);
+    /// Two CIM motors per drive side.
+    static constexpr frc::DCMotor DualCIMPerSide = frc::DCMotor::CIM(2);
+    /// One Mini CIM motor per drive side.
+    static constexpr frc::DCMotor SingleMiniCIMPerSide =
+        frc::DCMotor::MiniCIM(1);
+    /// Two Mini CIM motors per drive side.
+    static constexpr frc::DCMotor DualMiniCIMPerSide = frc::DCMotor::MiniCIM(2);
+    /// One Falcon 500 motor per drive side.
+    static constexpr frc::DCMotor SingleFalcon500PerSide =
+        frc::DCMotor::Falcon500(1);
+    /// Two Falcon 500 motors per drive side.
+    static constexpr frc::DCMotor DualFalcon500PerSide =
+        frc::DCMotor::Falcon500(2);
+    /// One NEO motor per drive side.
+    static constexpr frc::DCMotor SingleNEOPerSide = frc::DCMotor::NEO(1);
+    /// Two NEO motors per drive side.
+    static constexpr frc::DCMotor DualNEOPerSide = frc::DCMotor::NEO(2);
+  };
+
+  /**
+   * Represents common wheel sizes of the kit drivetrain.
+   */
+  class KitbotWheelSize {
+   public:
+    /// Six inch diameter wheels.
+    static constexpr units::meter_t kSixInch = 6_in;
+    /// Eight inch diameter wheels.
+    static constexpr units::meter_t kEightInch = 8_in;
+    /// Ten inch diameter wheels.
+    static constexpr units::meter_t kTenInch = 10_in;
+  };
+
+  /**
+   * 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 omitted 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.
+   */
+  static DifferentialDrivetrainSim CreateKitbotSim(
+      frc::DCMotor motor, double gearing, units::meter_t wheelSize,
+      const std::array<double, 7>& measurementStdDevs = {}) {
+    // MOI estimation -- note that I = mr² for point masses
+    units::kilogram_square_meter_t batteryMoi = 12.5_lb * 10_in * 10_in;
+    units::kilogram_square_meter_t gearboxMoi = (2.8_lb + 2.0_lb) *
+                                                2  // CIM plus toughbox per side
+                                                * (26_in / 2) * (26_in / 2);
+
+    return DifferentialDrivetrainSim{
+        motor,           gearing, batteryMoi + gearboxMoi, 60_lb,
+        wheelSize / 2.0, 26_in,   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. 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 omitted 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.
+   */
+  static DifferentialDrivetrainSim CreateKitbotSim(
+      frc::DCMotor motor, double gearing, units::meter_t wheelSize,
+      units::kilogram_square_meter_t J,
+      const std::array<double, 7>& measurementStdDevs = {}) {
+    return DifferentialDrivetrainSim{
+        motor, gearing, J, 60_lb, wheelSize / 2.0, 26_in, measurementStdDevs};
+  }
+
+ private:
+  /**
+   * Returns an element of the state vector.
+   *
+   * @param output The row of the output vector.
+   */
+  double GetOutput(int output) const;
+
+  /**
+   * Returns the current output vector y.
+   */
+  Vectord<7> GetOutput() const;
+
+  /**
+   * Returns an element of the state vector. Note that this will not include
+   * noise!
+   *
+   * @param output The row of the output vector.
+   */
+  double GetState(int state) const;
+
+  /**
+   * Returns the current state vector x. Note that this will not include noise!
+   */
+  Vectord<7> GetState() const;
+
+  LinearSystem<2, 2, 2> m_plant;
+  units::meter_t m_rb;
+  units::meter_t m_wheelRadius;
+
+  DCMotor m_motor;
+
+  double m_originalGearing;
+  double m_currentGearing;
+
+  Vectord<7> m_x;
+  Eigen::Vector2d m_u;
+  Vectord<7> m_y;
+  std::array<double, 7> m_measurementStdDevs;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/DigitalPWMSim.hpp

@@ -0,0 +1,136 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class DigitalOutput;
+
+namespace sim {
+
+/**
+ * Class to control a simulated digital PWM output.
+ *
+ * This is for duty cycle PWM outputs on a DigitalOutput, not for the servo
+ * style PWM outputs on a PWM channel.
+ */
+class DigitalPWMSim {
+ public:
+  /**
+   * Constructs from a DigitalOutput object.
+   *
+   * @param digitalOutput DigitalOutput to simulate
+   */
+  explicit DigitalPWMSim(const DigitalOutput& digitalOutput);
+
+  /**
+   * Creates an DigitalPWMSim for a digital I/O channel.
+   *
+   * @param channel DIO channel
+   * @return Simulated object
+   * @throws std::out_of_range if no Digital PWM is configured for that channel
+   */
+  static DigitalPWMSim CreateForChannel(int channel);
+
+  /**
+   * Creates an DigitalPWMSim for a simulated index.
+   * The index is incremented for each simulated DigitalPWM.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static DigitalPWMSim CreateForIndex(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check whether this PWM output has been initialized.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Define whether this PWM output has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
+  void SetInitialized(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterDutyCycleCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Read the duty cycle value.
+   *
+   * @return the duty cycle value of this PWM output
+   */
+  double GetDutyCycle() const;
+
+  /**
+   * Set the duty cycle value of this PWM output.
+   *
+   * @param dutyCycle the new value
+   */
+  void SetDutyCycle(double 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterPinCallback(NotifyCallback callback,
+                                                     bool initialNotify);
+
+  /**
+   * Check the pin number.
+   *
+   * @return the pin number
+   */
+  int GetPin() const;
+
+  /**
+   * Change the pin number.
+   *
+   * @param pin the new pin number
+   */
+  void SetPin(int pin);
+
+  /**
+   * Reset all simulation data.
+   */
+  void ResetData();
+
+ private:
+  explicit DigitalPWMSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/DoubleSolenoidSim.hpp

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/DoubleSolenoid.h"
+#include "frc/PneumaticsModuleType.h"
+#include "frc/simulation/PneumaticsBaseSim.h"
+
+namespace frc::sim {
+
+class DoubleSolenoidSim {
+ public:
+  DoubleSolenoidSim(std::shared_ptr<PneumaticsBaseSim> moduleSim, int fwd,
+                    int rev);
+  DoubleSolenoidSim(int module, PneumaticsModuleType type, int fwd, int rev);
+  DoubleSolenoidSim(PneumaticsModuleType type, int fwd, int rev);
+
+  DoubleSolenoid::Value Get() const;
+  void Set(DoubleSolenoid::Value output);
+
+  std::shared_ptr<PneumaticsBaseSim> GetModuleSim() const;
+
+ private:
+  std::shared_ptr<PneumaticsBaseSim> m_module;
+  int m_fwd;
+  int m_rev;
+};
+
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/DriverStationSim.hpp

@@ -0,0 +1,400 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/DriverStationTypes.h>
+
+#include "frc/DriverStation.h"
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc::sim {
+
+/**
+ * Class to control a simulated driver station.
+ */
+class DriverStationSim {
+ public:
+  /**
+   * 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterEnabledCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if the DS is enabled.
+   *
+   * @return true if enabled
+   */
+  static bool GetEnabled();
+
+  /**
+   * Change whether the DS is enabled.
+   *
+   * @param enabled the new value
+   */
+  static void SetEnabled(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterAutonomousCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if the DS is in autonomous.
+   *
+   * @return true if autonomous
+   */
+  static bool GetAutonomous();
+
+  /**
+   * Change whether the DS is in autonomous.
+   *
+   * @param autonomous the new value
+   */
+  static void SetAutonomous(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterTestCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if the DS is in test.
+   *
+   * @return true if test
+   */
+  static bool GetTest();
+
+  /**
+   * Change whether the DS is in test.
+   *
+   * @param test the new value
+   */
+  static void SetTest(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterEStopCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if eStop has been activated.
+   *
+   * @return true if eStopped
+   */
+  static bool GetEStop();
+
+  /**
+   * Set whether eStop is active.
+   *
+   * @param eStop true to activate
+   */
+  static void SetEStop(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterFmsAttachedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if the FMS is connected.
+   *
+   * @return true if FMS is connected
+   */
+  static bool GetFmsAttached();
+
+  /**
+   * Change whether the FMS is connected.
+   *
+   * @param fmsAttached the new value
+   */
+  static void SetFmsAttached(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterDsAttachedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if the DS is attached.
+   *
+   * @return true if attached
+   */
+  static bool GetDsAttached();
+
+  /**
+   * Change whether the DS is attached.
+   *
+   * @param dsAttached the new value
+   */
+  static void SetDsAttached(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterAllianceStationIdCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the alliance station ID (color + number).
+   *
+   * @return the alliance station color and number
+   */
+  static HAL_AllianceStationID GetAllianceStationId();
+
+  /**
+   * Change the alliance station.
+   *
+   * @param allianceStationId the new alliance station
+   */
+  static void SetAllianceStationId(HAL_AllianceStationID allianceStationId);
+
+  /**
+   * 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterMatchTimeCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the current value of the match timer.
+   *
+   * @return the current match time
+   */
+  static double GetMatchTime();
+
+  /**
+   * Sets the match timer.
+   *
+   * @param matchTime the new match time
+   */
+  static void SetMatchTime(double matchTime);
+
+  /**
+   * Updates DriverStation data so that new values are visible to the user
+   * program.
+   */
+  static void NotifyNewData();
+
+  /**
+   * Sets suppression of DriverStation::ReportError and ReportWarning messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  static void SetSendError(bool shouldSend);
+
+  /**
+   * Sets suppression of DriverStation::SendConsoleLine messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  static void SetSendConsoleLine(bool shouldSend);
+
+  /**
+   * Gets the joystick outputs.
+   *
+   * @param stick The joystick number
+   * @return The joystick outputs
+   */
+  static int64_t GetJoystickOutputs(int stick);
+
+  /**
+   * Gets the joystick rumble.
+   *
+   * @param stick The joystick number
+   * @param rumbleNum Rumble to get (0=left, 1=right)
+   * @return The joystick rumble value
+   */
+  static int GetJoystickRumble(int stick, int 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
+   */
+  static void SetJoystickButton(int stick, int button, bool 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
+   */
+  static void SetJoystickAxis(int stick, int axis, double 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
+   */
+  static void SetJoystickPOV(int stick, int pov,
+                             DriverStation::POVDirection value);
+
+  /**
+   * Sets the number of axes for a joystick.
+   *
+   * @param stick The joystick number
+   * @param maximumIndex The number of axes on the indicated joystick
+   */
+  static void SetJoystickAxesMaximumIndex(int stick, int maximumIndex);
+
+  /**
+   * Sets the number of axes for a joystick.
+   *
+   * @param stick The joystick number
+   * @param available The number of axes on the indicated joystick
+   */
+  static void SetJoystickAxesAvailable(int stick, int available);
+
+  /**
+   * Sets the number of POVs for a joystick.
+   *
+   * @param stick The joystick number
+   * @param maximumIndex The number of POVs on the indicated joystick
+   */
+  static void SetJoystickPOVsMaximumIndex(int stick, int maximumIndex);
+
+  /**
+   * Sets the number of POVs for a joystick.
+   *
+   * @param stick The joystick number
+   * @param available The number of POVs on the indicated joystick
+   */
+  static void SetJoystickPOVsAvailable(int stick, int available);
+
+  /**
+   * Sets the number of buttons for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of buttons on the indicated joystick
+   */
+  static void SetJoystickButtonsMaximumIndex(int stick, int count);
+
+  static void SetJoystickButtonsAvailable(int stick, uint64_t available);
+
+  /**
+   * Sets the value of isGamepad for a joystick.
+   *
+   * @param stick The joystick number
+   * @param isGamepad The value of isGamepad
+   */
+  static void SetJoystickIsGamepad(int stick, bool isGamepad);
+
+  /**
+   * Sets the value of type for a joystick.
+   *
+   * @param stick The joystick number
+   * @param type The value of type
+   */
+  static void SetJoystickType(int stick, int type);
+
+  /**
+   * Sets the name of a joystick.
+   *
+   * @param stick The joystick number
+   * @param name The value of name
+   */
+  static void SetJoystickName(int stick, std::string_view name);
+
+  /**
+   * Sets the game specific message.
+   *
+   * @param message the game specific message
+   */
+  static void SetGameSpecificMessage(std::string_view message);
+
+  /**
+   * Sets the event name.
+   *
+   * @param name the event name
+   */
+  static void SetEventName(std::string_view name);
+
+  /**
+   * Sets the match type.
+   *
+   * @param type the match type
+   */
+  static void SetMatchType(DriverStation::MatchType type);
+
+  /**
+   * Sets the match number.
+   *
+   * @param matchNumber the match number
+   */
+  static void SetMatchNumber(int matchNumber);
+
+  /**
+   * Sets the replay number.
+   *
+   * @param replayNumber the replay number
+   */
+  static void SetReplayNumber(int replayNumber);
+
+  /**
+   * Reset all simulation data for the Driver Station.
+   */
+  static void ResetData();
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/DutyCycleEncoderSim.hpp

@@ -0,0 +1,69 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/angle.h>
+
+namespace frc {
+
+class DutyCycleEncoder;
+
+namespace sim {
+
+/**
+ * Class to control a simulated duty cycle encoder.
+ */
+class DutyCycleEncoderSim {
+ public:
+  /**
+   * Constructs from a DutyCycleEncoder object.
+   *
+   * @param encoder DutyCycleEncoder to simulate
+   */
+  explicit DutyCycleEncoderSim(const DutyCycleEncoder& encoder);
+
+  /**
+   * Constructs from a digital input channel.
+   *
+   * @param channel digital input channel
+   */
+  explicit DutyCycleEncoderSim(int channel);
+
+  /**
+   * Get the position.
+   *
+   * @return The position.
+   */
+  double Get();
+
+  /**
+   * Set the position.
+   *
+   * @param value The position.
+   */
+  void Set(double value);
+
+  /**
+   * Get if the encoder is connected.
+   *
+   * @return true if the encoder is connected.
+   */
+  bool IsConnected();
+
+  /**
+   * Set if the encoder is connected.
+   *
+   * @param isConnected Whether or not the sensor is connected.
+   */
+  void SetConnected(bool isConnected);
+
+ private:
+  hal::SimDouble m_simPosition;
+  hal::SimBoolean m_simIsConnected;
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/DutyCycleSim.hpp

@@ -0,0 +1,125 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <units/frequency.h>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class DutyCycle;
+
+namespace sim {
+
+/**
+ * Class to control a simulated duty cycle digital input.
+ */
+class DutyCycleSim {
+ public:
+  /**
+   * Constructs from a DutyCycle object.
+   *
+   * @param dutyCycle DutyCycle to simulate
+   */
+  explicit DutyCycleSim(const DutyCycle& dutyCycle);
+
+  /**
+   * Creates a DutyCycleSim for a SmartIO channel.
+   *
+   * @param channel SmartIO channel
+   * @return Simulated object
+   */
+  static DutyCycleSim CreateForChannel(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check whether this duty cycle input has been initialized.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Define whether this duty cycle input has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
+  void SetInitialized(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterFrequencyCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Measure the frequency.
+   *
+   * @return the duty cycle frequency
+   */
+  units::hertz_t GetFrequency() const;
+
+  /**
+   * Change the duty cycle frequency.
+   *
+   * @param frequency the new frequency
+   */
+  void SetFrequency(units::hertz_t 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterOutputCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  /**
+   * Measure the output from this duty cycle port.
+   *
+   * @return the output value
+   */
+  double GetOutput() const;
+
+  /**
+   * Change the duty cycle output.
+   *
+   * @param output the new output value
+   */
+  void SetOutput(double output);
+
+  /**
+   * Reset all simulation data for the duty cycle output.
+   */
+  void ResetData();
+
+ private:
+  explicit DutyCycleSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/ElevatorSim.hpp

@@ -0,0 +1,179 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/velocity.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated elevator mechanism.
+ */
+class ElevatorSim : public LinearSystemSim<2, 1, 2> {
+ public:
+  template <typename Distance>
+  using Velocity_t = units::unit_t<
+      units::compound_unit<Distance, units::inverse<units::seconds>>>;
+
+  template <typename Distance>
+  using Acceleration_t = units::unit_t<units::compound_unit<
+      units::compound_unit<Distance, units::inverse<units::seconds>>,
+      units::inverse<units::seconds>>>;
+
+  /**
+   * Constructs a simulated elevator mechanism.
+   *
+   * @param plant              The linear system that represents the elevator.
+   *                           This system can be created with
+   *                           LinearSystemId::ElevatorSystem().
+   * @param gearbox            The type of and number of motors in your
+   *                           elevator gearbox.
+   * @param minHeight          The minimum allowed height of the elevator.
+   * @param maxHeight          The maximum allowed height of the elevator.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param startingHeight     The starting height of the elevator.
+   * @param measurementStdDevs The standard deviation of the measurements.
+   */
+  ElevatorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox,
+              units::meter_t minHeight, units::meter_t maxHeight,
+              bool simulateGravity, units::meter_t startingHeight,
+              const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
+
+  /**
+   * Constructs a simulated elevator mechanism.
+   *
+   * @param gearbox            The type of and number of motors in your
+   *                           elevator gearbox.
+   * @param gearing            The gearing of the elevator (numbers greater
+   *                           than 1 represent reductions).
+   * @param carriageMass       The mass of the elevator carriage.
+   * @param drumRadius         The radius of the drum that your cable is
+   *                           wrapped around.
+   * @param minHeight          The minimum allowed height of the elevator.
+   * @param maxHeight          The maximum allowed height of the elevator.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param startingHeight     The starting height of the elevator.
+   * @param measurementStdDevs The standard deviation of the measurements.
+   */
+  ElevatorSim(const DCMotor& gearbox, double gearing,
+              units::kilogram_t carriageMass, units::meter_t drumRadius,
+              units::meter_t minHeight, units::meter_t maxHeight,
+              bool simulateGravity, units::meter_t startingHeight,
+              const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
+
+  /**
+   * Constructs a simulated elevator mechanism.
+   *
+   * @param kV                 The velocity gain.
+   * @param kA                 The acceleration gain.
+   * @param gearbox            The type of and number of motors in your
+   *                           elevator gearbox.
+   * @param minHeight          The minimum allowed height of the elevator.
+   * @param maxHeight          The maximum allowed height of the elevator.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param startingHeight     The starting height of the elevator.
+   * @param measurementStdDevs The standard deviation of the measurements.
+   */
+  template <typename Distance>
+    requires std::same_as<units::meter, Distance> ||
+             std::same_as<units::radian, Distance>
+  ElevatorSim(decltype(1_V / Velocity_t<Distance>(1)) kV,
+              decltype(1_V / Acceleration_t<Distance>(1)) kA,
+              const DCMotor& gearbox, units::meter_t minHeight,
+              units::meter_t maxHeight, bool simulateGravity,
+              units::meter_t startingHeight,
+              const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
+  using LinearSystemSim::SetState;
+
+  /**
+   * Sets the elevator's state. The new position will be limited between the
+   * minimum and maximum allowed heights.
+   * @param position The new position
+   * @param velocity The new velocity
+   */
+  void SetState(units::meter_t position, units::meters_per_second_t velocity);
+
+  /**
+   * Returns whether the elevator would hit the lower limit.
+   *
+   * @param elevatorHeight The elevator height.
+   * @return Whether the elevator would hit the lower limit.
+   */
+  bool WouldHitLowerLimit(units::meter_t elevatorHeight) const;
+
+  /**
+   * Returns whether the elevator would hit the upper limit.
+   *
+   * @param elevatorHeight The elevator height.
+   * @return Whether the elevator would hit the upper limit.
+   */
+  bool WouldHitUpperLimit(units::meter_t elevatorHeight) const;
+
+  /**
+   * Returns whether the elevator has hit the lower limit.
+   *
+   * @return Whether the elevator has hit the lower limit.
+   */
+  bool HasHitLowerLimit() const;
+
+  /**
+   * Returns whether the elevator has hit the upper limit.
+   *
+   * @return Whether the elevator has hit the upper limit.
+   */
+  bool HasHitUpperLimit() const;
+
+  /**
+   * Returns the position of the elevator.
+   *
+   * @return The position of the elevator.
+   */
+  units::meter_t GetPosition() const;
+
+  /**
+   * Returns the velocity of the elevator.
+   *
+   * @return The velocity of the elevator.
+   */
+  units::meters_per_second_t GetVelocity() const;
+
+  /**
+   * Returns the elevator current draw.
+   *
+   * @return The elevator current draw.
+   */
+  units::ampere_t GetCurrentDraw() const;
+
+  /**
+   * Sets the input voltage for the elevator.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+ protected:
+  /**
+   * Updates the state estimate of the elevator.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (voltage).
+   * @param dt          The time difference between controller updates.
+   */
+  Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u,
+                     units::second_t dt) override;
+
+ private:
+  DCMotor m_gearbox;
+  units::meter_t m_minHeight;
+  units::meter_t m_maxHeight;
+  bool m_simulateGravity;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/EncoderSim.hpp

@@ -0,0 +1,320 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class Encoder;
+
+namespace sim {
+
+/**
+ * Class to control a simulated encoder.
+ */
+class EncoderSim {
+ public:
+  /**
+   * Constructs from an Encoder object.
+   *
+   * @param encoder Encoder to simulate
+   */
+  explicit EncoderSim(const Encoder& encoder);
+
+  /**
+   * 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
+   */
+  static EncoderSim CreateForChannel(int channel);
+
+  /**
+   * Creates an EncoderSim for a simulated index.
+   * The index is incremented for each simulated Encoder.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static EncoderSim CreateForIndex(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Read the Initialized value of the encoder.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Change the Initialized value of the encoder.
+   *
+   * @param initialized the new value
+   */
+  void SetInitialized(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterCountCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  /**
+   * Read the count of the encoder.
+   *
+   * @return the count
+   */
+  int GetCount() const;
+
+  /**
+   * Change the count of the encoder.
+   *
+   * @param count the new count
+   */
+  void SetCount(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterPeriodCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  /**
+   * Read the period of the encoder.
+   *
+   * @return the encoder period
+   */
+  double GetPeriod() const;
+
+  /**
+   * Change the encoder period.
+   *
+   * @param period the new period
+   */
+  void SetPeriod(double 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterResetCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  /**
+   * Check if the encoder has been reset.
+   *
+   * @return true if reset
+   */
+  bool GetReset() const;
+
+  /**
+   * Change the reset property of the encoder.
+   *
+   * @param reset the new value
+   */
+  void SetReset(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterMaxPeriodCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the max period of the encoder.
+   *
+   * @return the max period of the encoder
+   */
+  double GetMaxPeriod() const;
+
+  /**
+   * Change the max period of the encoder.
+   *
+   * @param maxPeriod the new value
+   */
+  void SetMaxPeriod(double 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterDirectionCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the direction of the encoder.
+   *
+   * @return the direction of the encoder
+   */
+  bool GetDirection() const;
+
+  /**
+   * Set the direction of the encoder.
+   *
+   * @param direction the new direction
+   */
+  void SetDirection(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterReverseDirectionCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the reverse direction of the encoder.
+   *
+   * @return the reverse direction of the encoder
+   */
+  bool GetReverseDirection() const;
+
+  /**
+   * Set the reverse direction.
+   *
+   * @param reverseDirection the new value
+   */
+  void SetReverseDirection(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterSamplesToAverageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the samples-to-average value.
+   *
+   * @return the samples-to-average value
+   */
+  int GetSamplesToAverage() const;
+
+  /**
+   * Set the samples-to-average value.
+   *
+   * @param samplesToAverage the new value
+   */
+  void SetSamplesToAverage(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterDistancePerPulseCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Read the distance per pulse of the encoder.
+   *
+   * @return the encoder distance per pulse
+   */
+  double GetDistancePerPulse() const;
+
+  /**
+   * Change the encoder distance per pulse.
+   *
+   * @param distancePerPulse the new distance per pulse
+   */
+  void SetDistancePerPulse(double distancePerPulse);
+
+  /**
+   * Resets all simulation data for this encoder.
+   */
+  void ResetData();
+
+  /**
+   * Change the encoder distance.
+   *
+   * @param distance the new distance
+   */
+  void SetDistance(double distance);
+
+  /**
+   * Read the distance of the encoder.
+   *
+   * @return the encoder distance
+   */
+  double GetDistance() const;
+
+  /**
+   * Change the rate of the encoder.
+   *
+   * @param rate the new rate
+   */
+  void SetRate(double rate);
+
+  /**
+   * Get the rate of the encoder.
+   *
+   * @return the rate of change
+   */
+  double GetRate() const;
+
+ private:
+  explicit EncoderSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/FlywheelSim.hpp

@@ -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.
+
+#pragma once
+
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+#include <units/moment_of_inertia.h>
+#include <units/torque.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated flywheel mechanism.
+ */
+class FlywheelSim : public LinearSystemSim<1, 1, 1> {
+ public:
+  /**
+   * Creates a simulated flywheel mechanism.
+   *
+   * @param plant              The linear system representing the flywheel. This
+   *                           system can be created with
+   *                           LinearSystemId::FlywheelSystem() or
+   * LinearSystemId::IdentifyVelocitySystem().
+   * @param gearbox            The type of and number of motors in the flywheel
+   *                           gearbox.
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   */
+  FlywheelSim(const LinearSystem<1, 1, 1>& plant, const DCMotor& gearbox,
+              const std::array<double, 1>& measurementStdDevs = {0.0});
+
+  using LinearSystemSim::SetState;
+
+  /**
+   * Sets the flywheel's angular velocity.
+   *
+   * @param velocity The new velocity
+   */
+  void SetVelocity(units::radians_per_second_t velocity);
+
+  /**
+   * Returns the flywheel's velocity.
+   *
+   * @return The flywheel's velocity.
+   */
+  units::radians_per_second_t GetAngularVelocity() const;
+
+  /**
+   * Returns the flywheel's acceleration.
+   *
+   * @return The flywheel's acceleration
+   */
+  units::radians_per_second_squared_t GetAngularAcceleration() const;
+
+  /**
+   * Returns the flywheel's torque.
+   *
+   * @return The flywheel's torque
+   */
+  units::newton_meter_t GetTorque() const;
+
+  /**
+   * Returns the flywheel's current draw.
+   *
+   * @return The flywheel's current draw.
+   */
+  units::ampere_t GetCurrentDraw() const;
+
+  /**
+   * Gets the input voltage for the flywheel.
+   *
+   * @return The flywheel input voltage.
+   */
+  units::volt_t GetInputVoltage() const;
+
+  /**
+   * Sets the input voltage for the flywheel.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+  /**
+   * Returns the gearbox.
+   */
+  DCMotor Gearbox() const { return m_gearbox; }
+
+  /**
+   * Returns the gearing;
+   */
+  double Gearing() const { return m_gearing; }
+
+  /**
+   * Returns the moment of inertia
+   */
+  units::kilogram_square_meter_t J() const { return m_j; }
+
+ private:
+  DCMotor m_gearbox;
+  double m_gearing;
+  units::kilogram_square_meter_t m_j;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/GamepadSim.hpp

@@ -0,0 +1,260 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/simulation/GenericHIDSim.h"
+
+namespace frc {
+
+class Gamepad;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Gamepad controller.
+ */
+class GamepadSim : public GenericHIDSim {
+ public:
+  /**
+   * Constructs from a Gamepad object.
+   *
+   * @param joystick controller to simulate
+   */
+  explicit GamepadSim(const Gamepad& joystick);
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit GamepadSim(int port);
+
+  /**
+   * Change the left X value of the controller's joystick.
+   *
+   * @param value the new value
+   */
+  void SetLeftX(double value);
+
+  /**
+   * Change the left Y value of the controller's joystick.
+   *
+   * @param value the new value
+   */
+  void SetLeftY(double value);
+
+  /**
+   * Change the right X value of the controller's joystick.
+   *
+   * @param value the new value
+   */
+  void SetRightX(double value);
+
+  /**
+   * Change the right Y value of the controller's joystick.
+   *
+   * @param value the new value
+   */
+  void SetRightY(double value);
+
+  /**
+   * Change the value of the left trigger axis on the controller.
+   *
+   * @param value the new value
+   */
+  void SetLeftTriggerAxis(double value);
+
+  /**
+   * Change the value of the right trigger axis on the controller.
+   *
+   * @param value the new value
+   */
+  void SetRightTriggerAxis(double value);
+
+  /**
+   * Change the value of the South Face button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetSouthFaceButton(bool value);
+
+  /**
+   * Change the value of the East Face button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetEastFaceButton(bool value);
+
+  /**
+   * Change the value of the West Face button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetWestFaceButton(bool value);
+
+  /**
+   * Change the value of the North Face button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetNorthFaceButton(bool value);
+
+  /**
+   * Change the value of the Back button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetBackButton(bool value);
+
+  /**
+   * Change the value of the Guide button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetGuideButton(bool value);
+
+  /**
+   * Change the value of the Start button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetStartButton(bool value);
+
+  /**
+   * Change the value of the left stick button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetLeftStickButton(bool value);
+
+  /**
+   * Change the value of the right stick button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetRightStickButton(bool value);
+
+  /**
+   * Change the value of the right shoulder button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetLeftShoulderButton(bool value);
+
+  /**
+   * Change the value of the right shoulder button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetRightShoulderButton(bool value);
+
+  /**
+   * Change the value of the D-pad up button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetDpadUpButton(bool value);
+
+  /**
+   * Change the value of the D-pad down button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetDpadDownButton(bool value);
+
+  /**
+   * Change the value of the D-pad left button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetDpadLeftButton(bool value);
+
+  /**
+   * Change the value of the D-pad right button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetDpadRightButton(bool value);
+
+  /**
+   * Change the value of the Miscellaneous 1 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetMisc1Button(bool value);
+
+  /**
+   * Change the value of the Right Paddle 1 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetRightPaddle1Button(bool value);
+
+  /**
+   * Change the value of the Left Paddle 1 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetLeftPaddle1Button(bool value);
+
+  /**
+   * Change the value of the Right Paddle 2 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetRightPaddle2Button(bool value);
+
+  /**
+   * Change the value of the Left Paddle 2 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetLeftPaddle2Button(bool value);
+
+  /**
+   * Change the value of the Touchpad button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetTouchpadButton(bool value);
+
+  /**
+   * Change the value of the Miscellaneous 2 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetMisc2Button(bool value);
+
+  /**
+   * Change the value of the Miscellaneous 3 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetMisc3Button(bool value);
+
+  /**
+   * Change the value of the Miscellaneous 4 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetMisc4Button(bool value);
+
+  /**
+   * Change the value of the Miscellaneous 5 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetMisc5Button(bool value);
+
+  /**
+   * Change the value of the Miscellaneous 6 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetMisc6Button(bool value);
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/GenericHIDSim.hpp

@@ -0,0 +1,143 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include "frc/DriverStation.h"
+#include "frc/GenericHID.h"
+
+namespace frc {
+
+class GenericHID;
+
+namespace sim {
+
+/**
+ * Class to control a simulated generic joystick.
+ */
+class GenericHIDSim {
+ public:
+  /**
+   * Constructs from a GenericHID object.
+   *
+   * @param joystick joystick to simulate
+   */
+  explicit GenericHIDSim(const GenericHID& joystick);
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit GenericHIDSim(int port);
+
+  /**
+   * Updates joystick data so that new values are visible to the user program.
+   */
+  void NotifyNewData();
+
+  /**
+   * Set the value of a given button.
+   *
+   * @param button the button to set
+   * @param value the new value
+   */
+  void SetRawButton(int button, bool value);
+
+  /**
+   * Set the value of a given axis.
+   *
+   * @param axis the axis to set
+   * @param value the new value
+   */
+  void SetRawAxis(int axis, double value);
+
+  /**
+   * Set the value of a given POV.
+   *
+   * @param pov the POV to set
+   * @param value the new value
+   */
+  void SetPOV(int pov, DriverStation::POVDirection value);
+
+  /**
+   * Set the value of the default POV (port 0).
+   *
+   * @param value the new value
+   */
+  void SetPOV(DriverStation::POVDirection value);
+
+  void SetAxesMaximumIndex(int maximumIndex);
+
+  /**
+   * Set the axis count of this device.
+   *
+   * @param count the new axis count
+   */
+  void SetAxesAvailable(int count);
+
+  void SetPOVsMaximumIndex(int maximumIndex);
+
+  /**
+   * Set the POV count of this device.
+   *
+   * @param count the new POV count
+   */
+  void SetPOVsAvailable(int count);
+
+  void SetButtonsMaximumIndex(int maximumIndex);
+
+  /**
+   * Set the button count of this device.
+   *
+   * @param count the new button count
+   */
+  void SetButtonsAvailable(uint64_t count);
+
+  /**
+   * Set the type of this device.
+   *
+   * @param type the new device type
+   */
+  void SetType(GenericHID::HIDType type);
+
+  /**
+   * Set the name of this device.
+   *
+   * @param name the new device name
+   */
+  void SetName(const char* name);
+
+  /**
+   * Read the output of a button.
+   *
+   * @param outputNumber the button number
+   * @return the value of the button (true = pressed)
+   */
+  bool GetOutput(int outputNumber);
+
+  /**
+   * Get the encoded 16-bit integer that passes button values.
+   *
+   * @return the button values
+   */
+  int64_t GetOutputs();
+
+  /**
+   * Get the joystick rumble.
+   *
+   * @param type the rumble to read
+   * @return the rumble value
+   */
+  double GetRumble(GenericHID::RumbleType type);
+
+ protected:
+  /// GenericHID port.
+  int m_port;
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/JoystickSim.hpp

@@ -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.
+
+#pragma once
+
+#include "frc/simulation/GenericHIDSim.h"
+
+namespace frc {
+
+class Joystick;
+
+namespace sim {
+
+/**
+ * Class to control a simulated joystick.
+ */
+class JoystickSim : public GenericHIDSim {
+ public:
+  /**
+   * Constructs from a Joystick object.
+   *
+   * @param joystick joystick to simulate
+   */
+  explicit JoystickSim(const Joystick& joystick);
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit JoystickSim(int port);
+
+  /**
+   * Set the X value of the joystick.
+   *
+   * @param value the new X value
+   */
+  void SetX(double value);
+
+  /**
+   * Set the Y value of the joystick.
+   *
+   * @param value the new Y value
+   */
+  void SetY(double value);
+
+  /**
+   * Set the Z value of the joystick.
+   *
+   * @param value the new Z value
+   */
+  void SetZ(double value);
+
+  /**
+   * Set the twist value of the joystick.
+   *
+   * @param value the new twist value
+   */
+  void SetTwist(double value);
+
+  /**
+   * Set the throttle value of the joystick.
+   *
+   * @param value the new throttle value
+   */
+  void SetThrottle(double value);
+
+  /**
+   * Set the trigger value of the joystick.
+   *
+   * @param state the new value
+   */
+  void SetTrigger(bool state);
+
+  /**
+   * Set the top state of the joystick.
+   *
+   * @param state the new state
+   */
+  void SetTop(bool state);
+
+ private:
+  const Joystick* m_joystick = nullptr;
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/LinearSystemSim.hpp

@@ -0,0 +1,167 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+
+#include <units/current.h>
+#include <units/time.h>
+
+#include "frc/EigenCore.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/LinearSystem.h"
+
+namespace frc::sim {
+/**
+ * This class helps simulate linear systems. To use this class, do the following
+ * in the simulationPeriodic() method.
+ *
+ * Call the SetInput() method with the inputs to your system (generally
+ * voltage). Call the Update() method to update the simulation. Set simulated
+ * sensor readings with the simulated positions in the GetOutput() method.
+ *
+ * @tparam States  Number of states of the system.
+ * @tparam Inputs  Number of inputs to the system.
+ * @tparam Outputs Number of outputs of the system.
+ */
+template <int States, int Inputs, int Outputs>
+class LinearSystemSim {
+ public:
+  /**
+   * Creates a simulated generic linear system.
+   *
+   * @param system             The system to simulate.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  explicit LinearSystemSim(
+      const LinearSystem<States, Inputs, Outputs>& system,
+      const std::array<double, Outputs>& measurementStdDevs = {})
+      : m_plant(system), m_measurementStdDevs(measurementStdDevs) {
+    m_x = Vectord<States>::Zero();
+    m_y = Vectord<Outputs>::Zero();
+    m_u = Vectord<Inputs>::Zero();
+  }
+
+  virtual ~LinearSystemSim() = default;
+
+  /**
+   * Updates the simulation.
+   *
+   * @param dt The time between updates.
+   */
+  void Update(units::second_t 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 noise. If the user did not pass a noise vector to the
+    // constructor, then this method will not do anything because
+    // the standard deviations default to zero.
+    m_y += frc::MakeWhiteNoiseVector<Outputs>(m_measurementStdDevs);
+  }
+
+  /**
+   * Returns the current output of the plant.
+   *
+   * @return The current output of the plant.
+   */
+  const Vectord<Outputs>& GetOutput() const { 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.
+   */
+  double GetOutput(int row) const { return m_y(row); }
+
+  /**
+   * Sets the system inputs (usually voltages).
+   *
+   * @param u The system inputs.
+   */
+  void SetInput(const Vectord<Inputs>& u) { 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.
+   */
+  void SetInput(int row, double value) { m_u(row, 0) = value; }
+
+  /**
+   * Returns the current input of the plant.
+   *
+   * @return The current input of the plant.
+   */
+  const Vectord<Inputs>& GetInput() const { 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.
+   */
+  double GetInput(int row) const { return m_u(row); }
+
+  /**
+   * Sets the system state.
+   *
+   * @param state The new state.
+   */
+  void SetState(const Vectord<States>& state) {
+    m_x = state;
+
+    // Update the output to reflect the new state.
+    //
+    //   yₖ = Cxₖ + Duₖ
+    m_y = m_plant.CalculateY(m_x, m_u);
+  }
+
+ protected:
+  /**
+   * 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.
+   */
+  virtual Vectord<States> UpdateX(const Vectord<States>& currentXhat,
+                                  const Vectord<Inputs>& u,
+                                  units::second_t dt) {
+    return m_plant.CalculateX(currentXhat, u, dt);
+  }
+
+  /**
+   * Clamp the input vector such that no element exceeds the given voltage. If
+   * any does, the relative magnitudes of the input will be maintained.
+   *
+   * @param maxInput The maximum magnitude of the input vector after clamping.
+   */
+  void ClampInput(double maxInput) {
+    m_u = frc::DesaturateInputVector<Inputs>(m_u, maxInput);
+  }
+
+  /// The plant that represents the linear system.
+  LinearSystem<States, Inputs, Outputs> m_plant;
+
+  /// State vector.
+  Vectord<States> m_x;
+
+  /// Input vector.
+  Vectord<Inputs> m_u;
+
+  /// Output vector.
+  Vectord<Outputs> m_y;
+
+  /// The standard deviations of measurements, used for adding noise to the
+  /// measurements.
+  std::array<double, Outputs> m_measurementStdDevs;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/PWMMotorControllerSim.hpp

@@ -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.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/length.h>
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+class PWMMotorController;
+
+namespace sim {
+
+class PWMMotorControllerSim {
+ public:
+  explicit PWMMotorControllerSim(const PWMMotorController& motorctrl);
+
+  explicit PWMMotorControllerSim(int channel);
+
+  double GetSpeed() const;
+
+ private:
+  hal::SimDouble m_simSpeed;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/PWMSim.hpp

@@ -0,0 +1,121 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class PWM;
+class PWMMotorController;
+
+namespace sim {
+
+/**
+ * Class to control a simulated PWM output.
+ */
+class PWMSim {
+ public:
+  /**
+   * Constructs from a PWM object.
+   *
+   * @param pwm PWM to simulate
+   */
+  explicit PWMSim(const PWM& pwm);
+
+  /**
+   * Constructs from a PWM channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit PWMSim(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check whether the PWM has been initialized.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Define whether the PWM has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
+  void SetInitialized(bool initialized);
+
+  /**
+   * Register a callback to be run when the PWM pulse microsecond value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterPulseMicrosecondCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the PWM pulse microsecond value.
+   *
+   * @return the PWM pulse microsecond value
+   */
+  int32_t GetPulseMicrosecond() const;
+
+  /**
+   * Set the PWM pulse microsecond value.
+   *
+   * @param microsecondPulseTime the PWM pulse microsecond value
+   */
+  void SetPulseMicrosecond(int32_t 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterOutputPeriodCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the PWM period scale.
+   *
+   * @return the PWM period scale
+   */
+  int GetOutputPeriod() const;
+
+  /**
+   * Set the PWM period scale.
+   *
+   * @param period the PWM period scale
+   */
+  void SetOutputPeriod(int period);
+
+  /**
+   * Reset all simulation data.
+   */
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/PneumaticsBaseSim.hpp

@@ -0,0 +1,198 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/PneumaticsBase.h"
+#include "frc/PneumaticsModuleType.h"
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc::sim {
+
+class PneumaticsBaseSim {
+ public:
+  virtual ~PneumaticsBaseSim() = default;
+
+  static std::shared_ptr<PneumaticsBaseSim> GetForType(
+      int module, PneumaticsModuleType type);
+
+  /**
+   * Check whether the PCM/PH has been initialized.
+   *
+   * @return true if initialized
+   */
+  virtual bool GetInitialized() const = 0;
+
+  /**
+   * Define whether the PCM/PH has been initialized.
+   *
+   * @param initialized true for initialized
+   */
+  virtual void SetInitialized(bool initialized) = 0;
+
+  /**
+   * 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.
+   * Save a reference to this object; it being deconstructed cancels the
+   * callback.
+   */
+  [[nodiscard]]
+  virtual std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify) = 0;
+
+  /**
+   * Check if the compressor is on.
+   *
+   * @return true if the compressor is active
+   */
+  virtual bool GetCompressorOn() const = 0;
+
+  /**
+   * Set whether the compressor is active.
+   *
+   * @param compressorOn the new value
+   */
+  virtual void SetCompressorOn(bool compressorOn) = 0;
+
+  /**
+   * 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.
+   * Save a reference to this object; it being deconstructed cancels the
+   * callback.
+   */
+  [[nodiscard]]
+  virtual std::unique_ptr<CallbackStore> RegisterCompressorOnCallback(
+      NotifyCallback callback, bool initialNotify) = 0;
+
+  /**
+   * Check the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @return the solenoid output
+   */
+  virtual bool GetSolenoidOutput(int channel) const = 0;
+
+  /**
+   * Change the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @param solenoidOutput the new solenoid output
+   */
+  virtual void SetSolenoidOutput(int channel, bool solenoidOutput) = 0;
+
+  /**
+   * 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.
+   * Save a reference to this object; it being deconstructed cancels the
+   * callback.
+   */
+  [[nodiscard]]
+  virtual std::unique_ptr<CallbackStore> RegisterSolenoidOutputCallback(
+      int channel, NotifyCallback callback, bool initialNotify) = 0;
+
+  /**
+   * Check the value of the pressure switch.
+   *
+   * @return the pressure switch value
+   */
+  virtual bool GetPressureSwitch() const = 0;
+
+  /**
+   * Set the value of the pressure switch.
+   *
+   * @param pressureSwitch the new value
+   */
+  virtual void SetPressureSwitch(bool pressureSwitch) = 0;
+
+  /**
+   * 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.
+   * Save a reference to this object; it being deconstructed cancels the
+   * callback.
+   */
+  [[nodiscard]]
+  virtual std::unique_ptr<CallbackStore> RegisterPressureSwitchCallback(
+      NotifyCallback callback, bool initialNotify) = 0;
+
+  /**
+   * Read the compressor current.
+   *
+   * @return the current of the compressor connected to this module
+   */
+  virtual double GetCompressorCurrent() const = 0;
+
+  /**
+   * Set the compressor current.
+   *
+   * @param compressorCurrent the new compressor current
+   */
+  virtual void SetCompressorCurrent(double compressorCurrent) = 0;
+
+  /**
+   * 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.
+   * Save a reference to this object; it being deconstructed cancels the
+   * callback.
+   */
+  [[nodiscard]]
+  virtual std::unique_ptr<CallbackStore> RegisterCompressorCurrentCallback(
+      NotifyCallback callback, bool initialNotify) = 0;
+
+  /**
+   * Get the current value of all solenoid outputs.
+   *
+   * @return the solenoid outputs (1 bit per output)
+   */
+  virtual uint8_t GetAllSolenoidOutputs() const = 0;
+
+  /**
+   * Change all of the solenoid outputs.
+   *
+   * @param outputs the new solenoid outputs (1 bit per output)
+   */
+  virtual void SetAllSolenoidOutputs(uint8_t outputs) = 0;
+
+  /** Reset all simulation data for this object. */
+  virtual void ResetData() = 0;
+
+ protected:
+  /// PneumaticsBase index.
+  const int m_index;
+
+  /**
+   * Constructs a PneumaticsBaseSim with the given index.
+   *
+   * @param index The index.
+   */
+  explicit PneumaticsBaseSim(const int index);
+
+  /**
+   * Constructs a PneumaticsBaseSim for the given module.
+   *
+   * @param module The module.
+   */
+  explicit PneumaticsBaseSim(const PneumaticsBase& module);
+};
+
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/PowerDistributionSim.hpp

@@ -0,0 +1,171 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class PowerDistribution;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Power Distribution Panel (PowerDistribution).
+ */
+class PowerDistributionSim {
+ public:
+  /**
+   * Constructs from a PowerDistribution module number (CAN ID).
+   *
+   * @param module module number
+   */
+  explicit PowerDistributionSim(int module = 0);
+
+  /**
+   * Constructs from a PowerDistribution object.
+   *
+   * @param pdp PowerDistribution to simulate
+   */
+  explicit PowerDistributionSim(const PowerDistribution& pdp);
+
+  /**
+   * Register a callback to be run when the PowerDistribution is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check whether the PowerDistribution has been initialized.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Define whether the PowerDistribution has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
+  void SetInitialized(bool initialized);
+
+  /**
+   * Register a callback to be run whenever the PowerDistribution temperature
+   * changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterTemperatureCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check the temperature of the PowerDistribution.
+   *
+   * @return the PowerDistribution temperature
+   */
+  double GetTemperature() const;
+
+  /**
+   * Define the PowerDistribution temperature.
+   *
+   * @param temperature the new PowerDistribution temperature
+   */
+  void SetTemperature(double temperature);
+
+  /**
+   * Register a callback to be run whenever the PowerDistribution voltage
+   * changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check the PowerDistribution voltage.
+   *
+   * @return the PowerDistribution voltage.
+   */
+  double GetVoltage() const;
+
+  /**
+   * Set the PowerDistribution voltage.
+   *
+   * @param voltage the new PowerDistribution voltage
+   */
+  void SetVoltage(double 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterCurrentCallback(
+      int channel, NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Read the current in one of the PowerDistribution channels.
+   *
+   * @param channel the channel to check
+   * @return the current in the given channel
+   */
+  double GetCurrent(int channel) const;
+
+  /**
+   * Change the current in the given channel.
+   *
+   * @param channel the channel to edit
+   * @param current the new current for the channel
+   */
+  void SetCurrent(int channel, double current);
+
+  /**
+   * Read the current of all of the PowerDistribution channels.
+   *
+   * @param currents output array; set to the current in each channel. The
+   *                 array must be big enough to hold all the PowerDistribution
+   *                 channels
+   * @param length   length of output array
+   */
+  void GetAllCurrents(double* currents, int length) const;
+
+  /**
+   * Change the current in all of the PowerDistribution channels.
+   *
+   * @param currents array containing the current values for each channel. The
+   *                 array must be big enough to hold all the PowerDistribution
+   *                 channels
+   * @param length   length of array
+   */
+  void SetAllCurrents(const double* currents, int length);
+
+  /**
+   * Reset all PowerDistribution simulation data.
+   */
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/REVPHSim.hpp

@@ -0,0 +1,123 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/PneumaticsBase.h"
+#include "frc/simulation/CallbackStore.h"
+#include "frc/simulation/PneumaticsBaseSim.h"
+
+namespace frc {
+
+class Compressor;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Pneumatic Control Module (PCM).
+ */
+class REVPHSim : public PneumaticsBaseSim {
+ public:
+  /**
+   * Constructs with the default PCM module number (CAN ID).
+   */
+  REVPHSim();
+
+  /**
+   * Constructs from a PCM module number (CAN ID).
+   *
+   * @param module module number
+   */
+  explicit REVPHSim(int module);
+
+  explicit REVPHSim(const PneumaticsBase& pneumatics);
+
+  ~REVPHSim() override = default;
+
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify) override;
+
+  bool GetInitialized() const override;
+
+  void SetInitialized(bool solenoidInitialized) override;
+
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterSolenoidOutputCallback(
+      int channel, NotifyCallback callback, bool initialNotify) override;
+
+  bool GetSolenoidOutput(int channel) const override;
+
+  void SetSolenoidOutput(int channel, bool solenoidOutput) override;
+
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterCompressorOnCallback(
+      NotifyCallback callback, bool initialNotify) override;
+
+  /**
+   * Check if the compressor is on.
+   *
+   * @return true if the compressor is active
+   */
+  bool GetCompressorOn() const override;
+
+  /**
+   * Set whether the compressor is active.
+   *
+   * @param compressorOn the new value
+   */
+  void SetCompressorOn(bool compressorOn) override;
+
+  /**
+   * 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterCompressorConfigTypeCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check whether the closed loop compressor control is active.
+   *
+   * @return compressor config type
+   */
+  int GetCompressorConfigType() const;
+
+  /**
+   * Turn on/off the closed loop control of the compressor.
+   *
+   * @param compressorConfigType compressor config type
+   */
+  void SetCompressorConfigType(int compressorConfigType);
+
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterPressureSwitchCallback(
+      NotifyCallback callback, bool initialNotify) override;
+
+  bool GetPressureSwitch() const override;
+
+  void SetPressureSwitch(bool pressureSwitch) override;
+
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterCompressorCurrentCallback(
+      NotifyCallback callback, bool initialNotify) override;
+
+  double GetCompressorCurrent() const override;
+
+  void SetCompressorCurrent(double compressorCurrent) override;
+
+  uint8_t GetAllSolenoidOutputs() const override;
+
+  void SetAllSolenoidOutputs(uint8_t outputs) override;
+
+  void ResetData() override;
+};
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/RoboRioSim.hpp

@@ -0,0 +1,262 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <units/current.h>
+#include <units/temperature.h>
+#include <units/voltage.h>
+
+#include "frc/RobotController.h"
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc::sim {
+
+/**
+ * A utility class to control a simulated RoboRIO.
+ */
+class RoboRioSim {
+ public:
+  /**
+   * 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterVInVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Measure the Vin voltage.
+   *
+   * @return the Vin voltage
+   */
+  static units::volt_t GetVInVoltage();
+
+  /**
+   * Define the Vin voltage.
+   *
+   * @param vInVoltage the new voltage
+   */
+  static void SetVInVoltage(units::volt_t 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterUserVoltage3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Measure the 3.3V rail voltage.
+   *
+   * @return the 3.3V rail voltage
+   */
+  static units::volt_t GetUserVoltage3V3();
+
+  /**
+   * Define the 3.3V rail voltage.
+   *
+   * @param userVoltage3V3 the new voltage
+   */
+  static void SetUserVoltage3V3(units::volt_t 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterUserCurrent3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Measure the 3.3V rail current.
+   *
+   * @return the 3.3V rail current
+   */
+  static units::ampere_t GetUserCurrent3V3();
+
+  /**
+   * Define the 3.3V rail current.
+   *
+   * @param userCurrent3V3 the new current
+   */
+  static void SetUserCurrent3V3(units::ampere_t 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterUserActive3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the 3.3V rail active state.
+   *
+   * @return true if the 3.3V rail is active
+   */
+  static bool GetUserActive3V3();
+
+  /**
+   * Set the 3.3V rail active state.
+   *
+   * @param userActive3V3 true to make rail active
+   */
+  static void SetUserActive3V3(bool 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterUserFaults3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the 3.3V rail number of faults.
+   *
+   * @return number of faults
+   */
+  static int GetUserFaults3V3();
+
+  /**
+   * Set the 3.3V rail number of faults.
+   *
+   * @param userFaults3V3 number of faults
+   */
+  static void SetUserFaults3V3(int 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterBrownoutVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Measure the brownout voltage.
+   *
+   * @return the brownout voltage
+   */
+  static units::volt_t GetBrownoutVoltage();
+
+  /**
+   * Define the brownout voltage.
+   *
+   * @param brownoutVoltage the new voltage
+   */
+  static void SetBrownoutVoltage(units::volt_t brownoutVoltage);
+
+  /**
+   * 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterCPUTempCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the cpu temp.
+   *
+   * @return the cpu temp.
+   */
+  static units::celsius_t GetCPUTemp();
+
+  /**
+   * Define the cpu temp.
+   *
+   * @param cpuTemp the new cpu temp.
+   */
+  static void SetCPUTemp(units::celsius_t 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 CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterTeamNumberCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the team number.
+   *
+   * @return the team number.
+   */
+  static int32_t GetTeamNumber();
+
+  /**
+   * Set the team number.
+   *
+   * @param teamNumber the new team number.
+   */
+  static void SetTeamNumber(int32_t teamNumber);
+
+  /**
+   * Get the serial number.
+   *
+   * @return The serial number.
+   */
+  static std::string GetSerialNumber();
+
+  /**
+   * Set the serial number.
+   *
+   * @param serialNumber The serial number.
+   */
+  static void SetSerialNumber(std::string_view serialNumber);
+
+  /**
+   * Get the comments.
+   *
+   * @return The comments.
+   */
+  static std::string GetComments();
+
+  /**
+   * Set the comments.
+   *
+   * @param comments The comments.
+   */
+  static void SetComments(std::string_view comments);
+
+  /**
+   * Reset all simulation data.
+   */
+  static void ResetData();
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/SendableChooserSim.hpp

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StringTopic.h>
+
+#include "frc/RobotBase.h"
+
+namespace frc::sim {
+
+/**
+ * Class that facilitates control of a SendableChooser's selected option in
+ * simulation.
+ */
+class SendableChooserSim {
+ public:
+  /**
+   * Constructs a SendableChooserSim.
+   *
+   * @param path The path where the SendableChooser is published.
+   */
+  explicit SendableChooserSim(std::string_view path);
+
+  /**
+   * Constructs a SendableChooserSim.
+   *
+   * @param inst The NetworkTables instance.
+   * @param path The path where the SendableChooser is published.
+   */
+  SendableChooserSim(nt::NetworkTableInstance inst, std::string_view path);
+
+  /**
+   * Set the selected option.
+   * @param option The option.
+   */
+  void SetSelected(std::string_view option);
+
+ private:
+  nt::StringPublisher m_publisher;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/SharpIRSim.hpp

@@ -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.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/length.h>
+
+#include "frc/SharpIR.h"
+
+namespace frc {
+
+/** Simulation class for Sharp IR sensors. */
+class SharpIRSim {
+ public:
+  /**
+   * Constructor.
+   *
+   * @param sharpIR The real sensor to simulate
+   */
+  explicit SharpIRSim(const SharpIR& sharpIR);
+
+  /**
+   * Constructor.
+   *
+   * @param channel Analog channel for this sensor
+   */
+  explicit SharpIRSim(int channel);
+
+  /**
+   * Set the range returned by the distance sensor.
+   *
+   * @param range range of the target returned by the sensor
+   */
+  void SetRange(units::meter_t range);
+
+ private:
+  hal::SimDouble m_simRange;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/simulation/SimDeviceSim.hpp

@@ -0,0 +1,162 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <string>
+#include <vector>
+
+#include <hal/SimDevice.h>
+#include <hal/simulation/SimDeviceData.h>
+
+namespace frc::sim {
+
+/**
+ * Class to control the simulation side of a SimDevice.
+ */
+class SimDeviceSim {
+ public:
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   */
+  explicit SimDeviceSim(const char* name);
+
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   * @param index device index number to append to name
+   */
+  SimDeviceSim(const char* name, int 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
+   */
+  SimDeviceSim(const char* name, int index, int channel);
+
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param handle the low level handle for the corresponding SimDevice.
+   */
+  explicit SimDeviceSim(HAL_SimDeviceHandle handle);
+
+  /**
+   * Get the name of this object.
+   *
+   * @return name
+   */
+  std::string GetName() const;
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  hal::SimValue GetValue(const char* name) const;
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  hal::SimInt GetInt(const char* name) const;
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  hal::SimLong GetLong(const char* name) const;
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  hal::SimDouble GetDouble(const char* name) const;
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  hal::SimEnum GetEnum(const char* name) const;
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  hal::SimBoolean GetBoolean(const char* name) const;
+
+  /**
+   * Get all options for the given enum.
+   *
+   * @param val the enum
+   * @return names of the different values for that enum
+   */
+  static std::vector<std::string> GetEnumOptions(hal::SimEnum val);
+
+  /**
+   * Get all properties.
+   *
+   * @param callback callback called for each property (SimValue).  Signature
+   *                 of the callback must be const char*, HAL_SimValueHandle,
+   *                 int, const HAL_Value*
+   */
+  template <typename F>
+  void EnumerateValues(F callback) const {
+    return HALSIM_EnumerateSimValues(
+        m_handle, &callback,
+        [](const char* name, void* param, HAL_SimValueHandle handle,
+           int direction, const struct HAL_Value* value) {
+          std::invoke(*static_cast<F*>(param), name, handle, direction, value);
+        });
+  }
+
+  /**
+   * Get the raw handle of this object.
+   *
+   * @return the handle used to refer to this object
+   */
+  operator HAL_SimDeviceHandle() const { return m_handle; }  // NOLINT
+
+  /**
+   * Get all sim devices with the given prefix.
+   *
+   * @param prefix the prefix to filter sim devices
+   * @param callback callback function to call for each sim device
+   */
+  template <typename F>
+  static void EnumerateDevices(const char* prefix, F callback) {
+    return HALSIM_EnumerateSimDevices(
+        prefix, &callback,
+        [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+          std::invoke(*static_cast<F*>(param), name, handle);
+        });
+  }
+
+  /**
+   * Reset all SimDevice data.
+   */
+  static void ResetData();
+
+ private:
+  HAL_SimDeviceHandle m_handle;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <hal/HALBase.h>
+#include <units/time.h>
+
+namespace frc::sim {
+
+/**
+ * Override the HAL runtime type (simulated/real).
+ *
+ * @param type runtime type
+ */
+void SetRuntimeType(HAL_RuntimeType type);
+
+void WaitForProgramStart();
+
+void SetProgramStarted();
+
+bool GetProgramStarted();
+
+/**
+ * Restart the simulator time.
+ */
+void RestartTiming();
+
+/**
+ * Pause the simulator time.
+ */
+void PauseTiming();
+
+/**
+ * Resume the simulator time.
+ */
+void ResumeTiming();
+
+/**
+ * Check if the simulator time is paused.
+ *
+ * @return true if paused
+ */
+bool IsTimingPaused();
+
+/**
+ * Advance the simulator time and wait for all notifiers to run.
+ *
+ * @param delta the amount to advance (in seconds)
+ */
+void StepTiming(units::second_t delta);
+
+/**
+ * Advance the simulator time and return immediately.
+ *
+ * @param delta the amount to advance (in seconds)
+ */
+void StepTimingAsync(units::second_t delta);
+
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/SingleJointedArmSim.hpp

@@ -0,0 +1,171 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/moment_of_inertia.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated arm mechanism.
+ */
+class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> {
+ public:
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param system             The system representing this arm. This system can
+   *                           be created with
+   *                           LinearSystemId::SingleJointedArmSystem().
+   * @param gearbox            The type and number of motors on the arm gearbox.
+   * @param gearing            The gear ratio of the arm (numbers greater than 1
+   *                           represent reductions).
+   * @param armLength          The length of the arm.
+   * @param minAngle           The minimum angle that the arm is capable of.
+   * @param maxAngle           The maximum angle that the arm is capable of.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param startingAngle      The initial position of the arm.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  SingleJointedArmSim(const LinearSystem<2, 1, 2>& system,
+                      const DCMotor& gearbox, double gearing,
+                      units::meter_t armLength, units::radian_t minAngle,
+                      units::radian_t maxAngle, bool simulateGravity,
+                      units::radian_t startingAngle,
+                      const std::array<double, 2>& measurementStdDevs = {0.0,
+                                                                         0.0});
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param gearbox            The type and number of motors on the arm gearbox.
+   * @param gearing            The gear ratio of the arm (numbers greater than 1
+   *                           represent reductions).
+   * @param moi                The moment of inertia of the arm. This can be
+   *                           calculated from CAD software.
+   * @param armLength          The length of the arm.
+   * @param minAngle           The minimum angle that the arm is capable of.
+   * @param maxAngle           The maximum angle that the arm is capable of.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param startingAngle      The initial position of the arm.
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   */
+  SingleJointedArmSim(const DCMotor& gearbox, double gearing,
+                      units::kilogram_square_meter_t moi,
+                      units::meter_t armLength, units::radian_t minAngle,
+                      units::radian_t maxAngle, bool simulateGravity,
+                      units::radian_t startingAngle,
+                      const std::array<double, 2>& measurementStdDevs = {0.0,
+                                                                         0.0});
+
+  using LinearSystemSim::SetState;
+
+  /**
+   * Sets the arm's state. The new angle will be limited between the minimum and
+   * maximum allowed limits.
+   *
+   * @param angle The new angle.
+   * @param velocity The new angular velocity.
+   */
+  void SetState(units::radian_t angle, units::radians_per_second_t velocity);
+
+  /**
+   * Returns whether the arm would hit the lower limit.
+   *
+   * @param armAngle The arm height.
+   * @return Whether the arm would hit the lower limit.
+   */
+  bool WouldHitLowerLimit(units::radian_t armAngle) const;
+
+  /**
+   * Returns whether the arm would hit the upper limit.
+   *
+   * @param armAngle The arm height.
+   * @return Whether the arm would hit the upper limit.
+   */
+  bool WouldHitUpperLimit(units::radian_t armAngle) const;
+
+  /**
+   * Returns whether the arm has hit the lower limit.
+   *
+   * @return Whether the arm has hit the lower limit.
+   */
+  bool HasHitLowerLimit() const;
+
+  /**
+   * Returns whether the arm has hit the upper limit.
+   *
+   * @return Whether the arm has hit the upper limit.
+   */
+  bool HasHitUpperLimit() const;
+
+  /**
+   * Returns the current arm angle.
+   *
+   * @return The current arm angle.
+   */
+  units::radian_t GetAngle() const;
+
+  /**
+   * Returns the current arm velocity.
+   *
+   * @return The current arm velocity.
+   */
+  units::radians_per_second_t GetVelocity() const;
+
+  /**
+   * Returns the arm current draw.
+   *
+   * @return The arm current draw.
+   */
+  units::ampere_t GetCurrentDraw() const;
+
+  /**
+   * Sets the input voltage for the arm.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+  /**
+   * Calculates a rough estimate of the moment of inertia of an arm given its
+   * length and mass.
+   *
+   * @param length The length of the arm.
+   * @param mass   The mass of the arm.
+   *
+   * @return The calculated moment of inertia.
+   */
+  static constexpr units::kilogram_square_meter_t EstimateMOI(
+      units::meter_t length, units::kilogram_t mass) {
+    return 1.0 / 3.0 * mass * length * length;
+  }
+
+ protected:
+  /**
+   * Updates the state estimate of the arm.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (voltage).
+   * @param dt          The time difference between controller updates.
+   */
+  Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u,
+                     units::second_t dt) override;
+
+ private:
+  units::meter_t m_armLen;
+  units::radian_t m_minAngle;
+  units::radian_t m_maxAngle;
+  const DCMotor m_gearbox;
+  double m_gearing;
+  bool m_simulateGravity;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/simulation/SolenoidSim.hpp

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/PneumaticsModuleType.h"
+#include "frc/simulation/PneumaticsBaseSim.h"
+
+namespace frc::sim {
+
+class SolenoidSim {
+ public:
+  SolenoidSim(std::shared_ptr<PneumaticsBaseSim> moduleSim, int channel);
+  SolenoidSim(int module, PneumaticsModuleType type, int channel);
+  SolenoidSim(PneumaticsModuleType type, int channel);
+  ~SolenoidSim() = default;
+
+  bool GetOutput() const;
+  void SetOutput(bool output);
+
+  /**
+   * Register a callback to be run when the output of this solenoid has changed.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   * Save a reference to this object; it being deconstructed cancels the
+   * callback.
+   */
+  [[nodiscard]]
+  std::unique_ptr<CallbackStore> RegisterOutputCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  std::shared_ptr<PneumaticsBaseSim> GetModuleSim() const;
+
+ private:
+  std::shared_ptr<PneumaticsBaseSim> m_module;
+  int m_channel;
+};
+
+}  // namespace frc::sim

wpilibc/src/main/native/include/wpi/smartdashboard/Field2d.hpp

@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string_view>
+#include <vector>
+
+#include <networktables/NTSendable.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <units/length.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/smartdashboard/FieldObject2d.h"
+
+namespace frc {
+
+/**
+ * 2D representation of game field for dashboards.
+ *
+ * An object's pose is the location shown on the dashboard view.  Note that
+ * for the robot, this may or may not match the internal odometry.  For example,
+ * if the robot is shown at a particular starting location, the pose in this
+ * class would represent the actual location on the field, but the robot's
+ * internal state might have a 0,0,0 pose (unless it's initialized to
+ * something different).
+ *
+ * As the user is able to edit the pose, code performing updates should get
+ * the robot pose, transform it as appropriate (e.g. based on wheel odometry),
+ * and set the new pose.
+ *
+ * This class provides methods to set the robot pose, but other objects can
+ * also be shown by using the GetObject() function.  Other objects can
+ * also have multiple poses (which will show the object at multiple locations).
+ */
+class Field2d : public nt::NTSendable, public wpi::SendableHelper<Field2d> {
+ public:
+  using Entry = size_t;
+
+  Field2d();
+
+  Field2d(Field2d&& rhs);
+  Field2d& operator=(Field2d&& rhs);
+
+  /**
+   * Set the robot pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  void SetRobotPose(const Pose2d& pose);
+
+  /**
+   * Set the robot pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  void SetRobotPose(units::meter_t x, units::meter_t y, Rotation2d rotation);
+
+  /**
+   * Get the robot pose.
+   *
+   * @return 2D pose
+   */
+  Pose2d GetRobotPose() const;
+
+  /**
+   * Get or create a field object.
+   *
+   * @return Field object
+   */
+  FieldObject2d* GetObject(std::string_view name);
+
+  /**
+   * Get the robot object.
+   *
+   * @return Field object for robot
+   */
+  FieldObject2d* GetRobotObject();
+
+  void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+  std::shared_ptr<nt::NetworkTable> m_table;
+
+  mutable wpi::mutex m_mutex;
+  std::vector<std::unique_ptr<FieldObject2d>> m_objects;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/smartdashboard/FieldObject2d.hpp

@@ -0,0 +1,110 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <initializer_list>
+#include <span>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <networktables/DoubleArrayTopic.h>
+#include <units/length.h>
+#include <wpi/SmallVector.h>
+#include <wpi/mutex.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+class Field2d;
+class Trajectory;
+
+/**
+ * Game field object on a Field2d.
+ */
+class FieldObject2d {
+  friend class Field2d;
+  struct private_init {};
+
+ public:
+  FieldObject2d(std::string_view name, const private_init&) : m_name{name} {}
+
+  FieldObject2d(FieldObject2d&& rhs);
+  FieldObject2d& operator=(FieldObject2d&& rhs);
+
+  /**
+   * Set the pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  void SetPose(const Pose2d& pose);
+
+  /**
+   * Set the pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  void SetPose(units::meter_t x, units::meter_t y, Rotation2d rotation);
+
+  /**
+   * Get the pose.
+   *
+   * @return 2D pose, or 0,0,0 if unknown / does not exist
+   */
+  Pose2d GetPose() const;
+
+  /**
+   * Set multiple poses from an array of Pose objects.
+   * The total number of poses is limited to 85.
+   *
+   * @param poses array of 2D poses
+   */
+  void SetPoses(std::span<const Pose2d> poses);
+
+  /**
+   * Set multiple poses from an array of Pose objects.
+   * The total number of poses is limited to 85.
+   *
+   * @param poses array of 2D poses
+   */
+  void SetPoses(std::initializer_list<Pose2d> poses);
+
+  /**
+   * Sets poses from a trajectory.
+   *
+   * @param trajectory The trajectory from which poses should be added.
+   */
+  void SetTrajectory(const Trajectory& trajectory);
+
+  /**
+   * Get multiple poses.
+   *
+   * @return vector of 2D poses
+   */
+  std::vector<Pose2d> GetPoses() const;
+
+  /**
+   * Get multiple poses.
+   *
+   * @param out output SmallVector to hold 2D poses
+   * @return span referring to output SmallVector
+   */
+  std::span<const Pose2d> GetPoses(wpi::SmallVectorImpl<Pose2d>& out) const;
+
+ private:
+  void UpdateEntry(bool setDefault = false);
+  void UpdateFromEntry() const;
+
+  mutable wpi::mutex m_mutex;
+  std::string m_name;
+  nt::DoubleArrayEntry m_entry;
+  mutable wpi::SmallVector<Pose2d, 1> m_poses;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/smartdashboard/ListenerExecutor.hpp

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+#include <wpi/mutex.h>
+
+namespace frc::detail {
+/**
+ * An executor for running listener tasks posted by Sendable listeners
+ * synchronously from the main application thread.
+ *
+ * @see Sendable
+ */
+class ListenerExecutor {
+ public:
+  /**
+   * Posts a task to the executor to be run synchronously from the main thread.
+   *
+   * @param task The task to run synchronously from the main thread.
+   */
+  void Execute(std::function<void()> task);
+
+  /**
+   * Runs all posted tasks.  Called periodically from main thread.
+   */
+  void RunListenerTasks();
+
+ private:
+  std::vector<std::function<void()>> m_tasks;
+  std::vector<std::function<void()>> m_runningTasks;
+  wpi::mutex m_lock;
+};
+}  // namespace frc::detail

wpilibc/src/main/native/include/wpi/smartdashboard/Mechanism2d.hpp

@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <networktables/DoubleArrayTopic.h>
+#include <networktables/NTSendable.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/StringTopic.h>
+#include <wpi/StringMap.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/smartdashboard/MechanismRoot2d.h"
+#include "frc/util/Color8Bit.h"
+
+namespace frc {
+
+/**
+ * Visual 2D representation of arms, elevators, and general mechanisms through
+ * a node-based API.
+ *
+ * A Mechanism2d object is published and contains at least one root node. A root
+ * is the anchor point of other nodes (such as ligaments). Other nodes are
+ * recursively appended based on other nodes.
+ *
+ * Except for the Mechanism2d container object, none of the objects should be
+ * passed or interacted with by value! Obtain pointers from factory methods such
+ * as Mechanism2d.GetRoot() and MechanismObject2d.Append<>(). The Mechanism2d
+ * container object owns the root nodes, and each node internally owns the nodes
+ * based on it. Beware not to let the Mechanism2d object out of scope - all
+ * nodes will be recursively destructed!
+ *
+ * @see MechanismObject2d
+ * @see MechanismLigament2d
+ * @see MechanismRoot2d
+ */
+class Mechanism2d : public nt::NTSendable,
+                    public wpi::SendableHelper<Mechanism2d> {
+ public:
+  /**
+   * Create a new Mechanism2d with the given dimensions and background color.
+   *
+   * The dimensions represent the canvas that all the nodes are drawn on. The
+   * default color is dark blue.
+   *
+   * @param width the width
+   * @param height the height
+   * @param backgroundColor the background color
+   */
+  Mechanism2d(double width, double height,
+              const Color8Bit& backgroundColor = {0, 0, 32});
+
+  /**
+   * Get or create a root in this Mechanism2d with the given name and
+   * position.
+   *
+   * <p>If a root with the given name already exists, the given x and y
+   * coordinates are not used.
+   *
+   * @param name the root name
+   * @param x the root x coordinate
+   * @param y the root y coordinate
+   * @return a new root object, or the existing one with the given name.
+   */
+  MechanismRoot2d* GetRoot(std::string_view name, double x, double y);
+
+  /**
+   * Set the Mechanism2d background color.
+   *
+   * @param color the new background color
+   */
+  void SetBackgroundColor(const Color8Bit& color);
+
+  void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+  double m_width;
+  double m_height;
+  std::string m_color;
+  mutable wpi::mutex m_mutex;
+  std::shared_ptr<nt::NetworkTable> m_table;
+  wpi::StringMap<MechanismRoot2d> m_roots;
+  nt::DoubleArrayPublisher m_dimsPub;
+  nt::StringPublisher m_colorPub;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/smartdashboard/MechanismLigament2d.hpp

@@ -0,0 +1,103 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string_view>
+
+#include <networktables/DoubleTopic.h>
+#include <networktables/StringTopic.h>
+#include <units/angle.h>
+
+#include "frc/smartdashboard/MechanismObject2d.h"
+#include "frc/util/Color8Bit.h"
+
+namespace frc {
+
+/**
+ * Ligament node on a Mechanism2d.
+ *
+ * A ligament can have its length changed (like an elevator) or angle changed,
+ * like an arm.
+ *
+ * @see Mechanism2d
+ */
+class MechanismLigament2d : public MechanismObject2d {
+ public:
+  MechanismLigament2d(std::string_view name, double length,
+                      units::degree_t angle, double lineWidth = 6,
+                      const frc::Color8Bit& color = {235, 137, 52});
+
+  /**
+   * Set the ligament color.
+   *
+   * @param color the color of the line
+   */
+  void SetColor(const Color8Bit& color);
+
+  /**
+   * Get the ligament color.
+   *
+   * @return the color of the line
+   */
+  Color8Bit GetColor();
+
+  /**
+   * Set the ligament's length.
+   *
+   * @param length the line length
+   */
+  void SetLength(double length);
+
+  /**
+   * Get the ligament length.
+   *
+   * @return the line length
+   */
+  double GetLength();
+
+  /**
+   * Set the ligament's angle relative to its parent.
+   *
+   * @param angle the angle
+   */
+  void SetAngle(units::degree_t angle);
+
+  /**
+   * Get the ligament's angle relative to its parent.
+   *
+   * @return the angle
+   */
+  double GetAngle();
+
+  /**
+   * Set the line thickness.
+   *
+   * @param lineWidth the line thickness
+   */
+  void SetLineWeight(double lineWidth);
+
+  /**
+   * Get the line thickness.
+   *
+   * @return the line thickness
+   */
+  double GetLineWeight();
+
+ protected:
+  void UpdateEntries(std::shared_ptr<nt::NetworkTable> table) override;
+
+ private:
+  nt::StringPublisher m_typePub;
+  double m_length;
+  nt::DoubleEntry m_lengthEntry;
+  double m_angle;
+  nt::DoubleEntry m_angleEntry;
+  double m_weight;
+  nt::DoubleEntry m_weightEntry;
+  char m_color[10];
+  nt::StringEntry m_colorEntry;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/smartdashboard/MechanismObject2d.hpp

@@ -0,0 +1,90 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <string_view>
+#include <utility>
+
+#include <networktables/NetworkTable.h>
+#include <wpi/StringMap.h>
+
+#include "frc/Errors.h"
+
+namespace frc {
+
+/**
+ * Common base class for all Mechanism2d node types.
+ *
+ * To append another node, call Append with the type of node and its
+ * construction parameters. None of the node types are designed to be
+ * constructed directly, and are owned by their parent node/container - obtain
+ * pointers from the Append function or similar factory methods.
+ *
+ * @see Mechanism2d.
+ */
+class MechanismObject2d {
+  friend class Mechanism2d;
+
+ protected:
+  explicit MechanismObject2d(std::string_view name);
+
+  /**
+   * Update all entries with new ones from a new table.
+   *
+   * @param table the new table.
+   */
+  virtual void UpdateEntries(std::shared_ptr<nt::NetworkTable> table) = 0;
+
+  mutable wpi::mutex m_mutex;
+
+ public:
+  virtual ~MechanismObject2d() = default;
+
+  /**
+   * Retrieve the object's name.
+   *
+   * @return the object's name relative to its parent.
+   */
+  const std::string& GetName() const;
+
+  /**
+   * Append a Mechanism object that is based on this one.
+   *
+   * @param name the name of the new object.
+   * @param args constructor arguments of the object type.
+   * @return the constructed and appended object, useful for variable
+   * assignments and call chaining.
+   * @throw if an object with the given name already exists.
+   */
+  template <typename T, typename... Args>
+    requires std::convertible_to<T*, MechanismObject2d*>
+  T* Append(std::string_view name, Args&&... args) {
+    std::scoped_lock lock(m_mutex);
+    auto& obj = m_objects[name];
+    if (obj) {
+      throw FRC_MakeError(
+          err::Error,
+          "MechanismObject names must be unique! `{}` was inserted twice!",
+          name);
+    }
+    obj = std::make_unique<T>(name, std::forward<Args>(args)...);
+    T* ex = static_cast<T*>(obj.get());
+    if (m_table) {
+      ex->Update(m_table->GetSubTable(name));
+    }
+    return ex;
+  }
+
+ private:
+  std::string m_name;
+  wpi::StringMap<std::unique_ptr<MechanismObject2d>> m_objects;
+  std::shared_ptr<nt::NetworkTable> m_table;
+  void Update(std::shared_ptr<nt::NetworkTable> table);
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/smartdashboard/MechanismRoot2d.hpp

@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string_view>
+
+#include <networktables/DoubleTopic.h>
+
+#include "MechanismObject2d.h"
+
+namespace frc {
+
+/**
+ * Root Mechanism2d node.
+ *
+ * A root is the anchor point of other nodes (such as ligaments).
+ *
+ * Do not create objects of this class directly! Obtain pointers from the
+ * Mechanism2d.GetRoot() factory method.
+ *
+ * <p>Append other nodes by using Append().
+ */
+class MechanismRoot2d : private MechanismObject2d {
+  friend class Mechanism2d;
+  struct private_init {};
+
+ public:
+  MechanismRoot2d(std::string_view name, double x, double y,
+                  const private_init&);
+
+  /**
+   * Set the root's position.
+   *
+   * @param x new x coordinate
+   * @param y new y coordinate
+   */
+  void SetPosition(double x, double y);
+
+  using MechanismObject2d::GetName;
+
+  using MechanismObject2d::Append;
+
+ private:
+  void UpdateEntries(std::shared_ptr<nt::NetworkTable> table) override;
+  inline void Flush();
+  double m_x;
+  double m_y;
+  nt::DoublePublisher m_xPub;
+  nt::DoublePublisher m_yPub;
+};
+}  // namespace frc

wpilibc/src/main/native/include/wpi/smartdashboard/SendableBuilderImpl.hpp

@@ -0,0 +1,238 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <span>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include <networktables/BooleanTopic.h>
+#include <networktables/NTSendableBuilder.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/StringTopic.h>
+#include <wpi/FunctionExtras.h>
+#include <wpi/SmallVector.h>
+
+namespace frc {
+
+/**
+ * Implementation detail for SendableBuilder.
+ */
+class SendableBuilderImpl : public nt::NTSendableBuilder {
+ public:
+  SendableBuilderImpl() = default;
+  ~SendableBuilderImpl() override = default;
+
+  SendableBuilderImpl(SendableBuilderImpl&&) = default;
+  SendableBuilderImpl& operator=(SendableBuilderImpl&&) = default;
+
+  /**
+   * Set the network table.  Must be called prior to any Add* functions being
+   * called.
+   * @param table Network table
+   */
+  void SetTable(std::shared_ptr<nt::NetworkTable> table);
+
+  /**
+   * Get the network table.
+   * @return The network table
+   */
+  std::shared_ptr<nt::NetworkTable> GetTable() override;
+
+  /**
+   * Return whether this sendable has an associated table.
+   * @return True if it has a table, false if not.
+   */
+  bool IsPublished() const override;
+
+  /**
+   * Return whether this sendable should be treated as an actuator.
+   * @return True if actuator, false if not.
+   */
+  bool IsActuator() const;
+
+  /**
+   * Synchronize with network table values by calling the getters for all
+   * properties and setters when the network table value has changed.
+   */
+  void Update() override;
+
+  /**
+   * Hook setters for all properties.
+   */
+  void StartListeners();
+
+  /**
+   * Unhook setters for all properties.
+   */
+  void StopListeners();
+
+  /**
+   * Clear properties.
+   */
+  void ClearProperties() override;
+
+  void SetSmartDashboardType(std::string_view type) override;
+  void SetActuator(bool value) override;
+  void SetUpdateTable(wpi::unique_function<void()> func) override;
+  nt::Topic GetTopic(std::string_view key) override;
+
+  void AddBooleanProperty(std::string_view key, std::function<bool()> getter,
+                          std::function<void(bool)> setter) override;
+
+  void PublishConstBoolean(std::string_view key, bool value) override;
+
+  void AddIntegerProperty(std::string_view key, std::function<int64_t()> getter,
+                          std::function<void(int64_t)> setter) override;
+
+  void PublishConstInteger(std::string_view key, int64_t value) override;
+
+  void AddFloatProperty(std::string_view key, std::function<float()> getter,
+                        std::function<void(float)> setter) override;
+
+  void PublishConstFloat(std::string_view key, float value) override;
+
+  void AddDoubleProperty(std::string_view key, std::function<double()> getter,
+                         std::function<void(double)> setter) override;
+
+  void PublishConstDouble(std::string_view key, double value) override;
+
+  void AddStringProperty(std::string_view key,
+                         std::function<std::string()> getter,
+                         std::function<void(std::string_view)> setter) override;
+
+  void PublishConstString(std::string_view key,
+                          std::string_view value) override;
+
+  void AddBooleanArrayProperty(
+      std::string_view key, std::function<std::vector<int>()> getter,
+      std::function<void(std::span<const int>)> setter) override;
+
+  void PublishConstBooleanArray(std::string_view key,
+                                std::span<const int> value) override;
+
+  void AddIntegerArrayProperty(
+      std::string_view key, std::function<std::vector<int64_t>()> getter,
+      std::function<void(std::span<const int64_t>)> setter) override;
+
+  void PublishConstIntegerArray(std::string_view key,
+                                std::span<const int64_t> value) override;
+
+  void AddFloatArrayProperty(
+      std::string_view key, std::function<std::vector<float>()> getter,
+      std::function<void(std::span<const float>)> setter) override;
+
+  void PublishConstFloatArray(std::string_view key,
+                              std::span<const float> value) override;
+
+  void AddDoubleArrayProperty(
+      std::string_view key, std::function<std::vector<double>()> getter,
+      std::function<void(std::span<const double>)> setter) override;
+
+  void PublishConstDoubleArray(std::string_view key,
+                               std::span<const double> value) override;
+
+  void AddStringArrayProperty(
+      std::string_view key, std::function<std::vector<std::string>()> getter,
+      std::function<void(std::span<const std::string>)> setter) override;
+
+  void PublishConstStringArray(std::string_view key,
+                               std::span<const std::string> value) override;
+
+  void AddRawProperty(
+      std::string_view key, std::string_view typeString,
+      std::function<std::vector<uint8_t>()> getter,
+      std::function<void(std::span<const uint8_t>)> setter) override;
+
+  void PublishConstRaw(std::string_view key, std::string_view typeString,
+                       std::span<const uint8_t> value) override;
+
+  void AddSmallStringProperty(
+      std::string_view key,
+      std::function<std::string_view(wpi::SmallVectorImpl<char>& buf)> getter,
+      std::function<void(std::string_view)> setter) override;
+
+  void AddSmallBooleanArrayProperty(
+      std::string_view key,
+      std::function<std::span<const int>(wpi::SmallVectorImpl<int>& buf)>
+          getter,
+      std::function<void(std::span<const int>)> setter) override;
+
+  void AddSmallIntegerArrayProperty(
+      std::string_view key,
+      std::function<
+          std::span<const int64_t>(wpi::SmallVectorImpl<int64_t>& buf)>
+          getter,
+      std::function<void(std::span<const int64_t>)> setter) override;
+
+  void AddSmallFloatArrayProperty(
+      std::string_view key,
+      std::function<std::span<const float>(wpi::SmallVectorImpl<float>& buf)>
+          getter,
+      std::function<void(std::span<const float>)> setter) override;
+
+  void AddSmallDoubleArrayProperty(
+      std::string_view key,
+      std::function<std::span<const double>(wpi::SmallVectorImpl<double>& buf)>
+          getter,
+      std::function<void(std::span<const double>)> setter) override;
+
+  void AddSmallStringArrayProperty(
+      std::string_view key,
+      std::function<
+          std::span<const std::string>(wpi::SmallVectorImpl<std::string>& buf)>
+          getter,
+      std::function<void(std::span<const std::string>)> setter) override;
+
+  void AddSmallRawProperty(
+      std::string_view key, std::string_view typeString,
+      std::function<std::span<uint8_t>(wpi::SmallVectorImpl<uint8_t>& buf)>
+          getter,
+      std::function<void(std::span<const uint8_t>)> setter) override;
+
+ private:
+  struct Property {
+    virtual ~Property() = default;
+    virtual void Update(bool controllable, int64_t time) = 0;
+  };
+
+  template <typename Topic>
+  struct PropertyImpl : public Property {
+    void Update(bool controllable, int64_t time) override;
+
+    using Publisher = typename Topic::PublisherType;
+    using Subscriber = typename Topic::SubscriberType;
+    Publisher pub;
+    Subscriber sub;
+    std::function<void(Publisher& pub, int64_t time)> updateNetwork;
+    std::function<void(Subscriber& sub)> updateLocal;
+  };
+
+  template <typename Topic, typename Getter, typename Setter>
+  void AddPropertyImpl(Topic topic, Getter getter, Setter setter);
+
+  template <typename Topic, typename Value>
+  void PublishConstImpl(Topic topic, Value value);
+
+  template <typename T, size_t Size, typename Topic, typename Getter,
+            typename Setter>
+  void AddSmallPropertyImpl(Topic topic, Getter getter, Setter setter);
+
+  std::vector<std::unique_ptr<Property>> m_properties;
+  std::vector<wpi::unique_function<void()>> m_updateTables;
+  std::shared_ptr<nt::NetworkTable> m_table;
+  bool m_controllable = false;
+  bool m_actuator = false;
+
+  nt::BooleanPublisher m_controllablePublisher;
+  nt::StringPublisher m_typePublisher;
+  nt::BooleanPublisher m_actuatorPublisher;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/smartdashboard/SendableChooser.hpp

@@ -0,0 +1,179 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <concepts>
+#include <functional>
+#include <memory>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include <wpi/StringMap.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/smartdashboard/SendableChooserBase.h"
+
+namespace frc {
+
+/**
+ * The SendableChooser class is a useful tool for presenting a selection of
+ * options to the SmartDashboard.
+ *
+ * For instance, you may wish to be able to select between multiple autonomous
+ * modes. You can do this by putting every possible Command you want to run as
+ * an autonomous into a SendableChooser and then put it into the SmartDashboard
+ * to have a list of options appear on the laptop. Once autonomous starts,
+ * simply ask the SendableChooser what the selected value is.
+ *
+ * @tparam T The type of values to be stored
+ * @see SmartDashboard
+ */
+template <class T>
+  requires std::copy_constructible<T> && std::default_initializable<T>
+class SendableChooser : public SendableChooserBase {
+  wpi::StringMap<T> m_choices;
+  std::function<void(T)> m_listener;
+  template <class U>
+  static U _unwrap_smart_ptr(const U& value) {
+    return value;
+  }
+
+  template <class U>
+  static std::weak_ptr<U> _unwrap_smart_ptr(const std::shared_ptr<U>& value) {
+    return value;
+  }
+
+ public:
+  using CopyType = decltype(_unwrap_smart_ptr(m_choices.find("")->second));
+
+  SendableChooser() = default;
+  ~SendableChooser() override = default;
+  SendableChooser(SendableChooser&& rhs) = default;
+  SendableChooser& operator=(SendableChooser&& rhs) = default;
+
+  /**
+   * Adds the given object to the list of options.
+   *
+   * On the SmartDashboard on the desktop, the object will appear as the given
+   * name.
+   *
+   * @param name   the name of the option
+   * @param object the option
+   */
+  void AddOption(std::string_view name, T object) {
+    m_choices[name] = std::move(object);
+  }
+
+  /**
+   * Add the given object to the list of options and marks it as the default.
+   *
+   * Functionally, this is very close to AddOption() except that it will use
+   * this as the default option if none other is explicitly selected.
+   *
+   * @param name   the name of the option
+   * @param object the option
+   */
+  void SetDefaultOption(std::string_view name, T object) {
+    m_defaultChoice = name;
+    AddOption(name, std::move(object));
+  }
+
+  /**
+   * Returns a copy of the selected option (a std::weak_ptr&lt;U&gt; if T =
+   * std::shared_ptr&lt;U&gt;).
+   *
+   * If there is none selected, it will return the default. If there is none
+   * selected and no default, then it will return a value-initialized instance.
+   * For integer types, this is 0. For container types like std::string, this is
+   * an empty string.
+   *
+   * @return The option selected
+   */
+  CopyType GetSelected() const {
+    std::string selected = m_defaultChoice;
+    {
+      std::scoped_lock lock(m_mutex);
+      if (m_haveSelected) {
+        selected = m_selected;
+      }
+    }
+    if (selected.empty()) {
+      return CopyType{};
+    } else {
+      auto it = m_choices.find(selected);
+      if (it == m_choices.end()) {
+        return CopyType{};
+      }
+      return _unwrap_smart_ptr(it->second);
+    }
+  }
+
+  /**
+   * Bind a listener that's called when the selected value changes.
+   * Only one listener can be bound. Calling this function will replace the
+   * previous listener.
+   * @param listener The function to call that accepts the new value
+   */
+  void OnChange(std::function<void(T)> listener) {
+    std::scoped_lock lock(m_mutex);
+    m_listener = listener;
+  }
+
+  void InitSendable(wpi::SendableBuilder& builder) override {
+    builder.SetSmartDashboardType("String Chooser");
+    builder.PublishConstInteger(kInstance, m_instance);
+    builder.AddStringArrayProperty(
+        kOptions,
+        [=, this] {
+          std::vector<std::string> keys;
+          for (const auto& choice : m_choices) {
+            keys.emplace_back(choice.first);
+          }
+          return keys;
+        },
+        nullptr);
+    builder.AddSmallStringProperty(
+        kDefault,
+        [=, this](wpi::SmallVectorImpl<char>&) -> std::string_view {
+          return m_defaultChoice;
+        },
+        nullptr);
+    builder.AddSmallStringProperty(
+        kActive,
+        [=, this](wpi::SmallVectorImpl<char>& buf) -> std::string_view {
+          std::scoped_lock lock(m_mutex);
+          if (m_haveSelected) {
+            buf.assign(m_selected.begin(), m_selected.end());
+            return {buf.data(), buf.size()};
+          } else {
+            return m_defaultChoice;
+          }
+        },
+        nullptr);
+    builder.AddStringProperty(kSelected, nullptr,
+                              [=, this](std::string_view val) {
+                                T choice{};
+                                std::function<void(T)> listener;
+                                {
+                                  std::scoped_lock lock(m_mutex);
+                                  m_haveSelected = true;
+                                  m_selected = val;
+                                  if (m_previousVal != val && m_listener) {
+                                    choice = m_choices[val];
+                                    listener = m_listener;
+                                  }
+                                  m_previousVal = val;
+                                }
+                                if (listener) {
+                                  listener(choice);
+                                }
+                              });
+  }
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/wpi/smartdashboard/SendableChooserBase.hpp

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <atomic>
+#include <string>
+
+#include <wpi/mutex.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+
+/**
+ * This class is a non-template base class for SendableChooser.
+ *
+ * It contains static, non-templated variables to avoid their duplication in the
+ * template class.
+ */
+class SendableChooserBase : public wpi::Sendable,
+                            public wpi::SendableHelper<SendableChooserBase> {
+ public:
+  SendableChooserBase();
+  ~SendableChooserBase() override = default;
+
+  SendableChooserBase(SendableChooserBase&& oth);
+  SendableChooserBase& operator=(SendableChooserBase&& oth);
+
+ protected:
+  static constexpr const char* kDefault = "default";
+  static constexpr const char* kOptions = "options";
+  static constexpr const char* kSelected = "selected";
+  static constexpr const char* kActive = "active";
+  static constexpr const char* kInstance = ".instance";
+
+  std::string m_defaultChoice;
+  std::string m_selected;
+  bool m_haveSelected = false;
+  mutable wpi::mutex m_mutex;
+  int m_instance;
+  std::string m_previousVal;
+  static std::atomic_int s_instances;
+};
+
+}  // namespace frc