SCRIPT Move cc files

wpilibc/src/main/native/cpp/hardware/accelerometer/ADXL345_I2C.cpp

@@ -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.
+
+#include "frc/ADXL345_I2C.h"
+
+#include <hal/UsageReporting.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NTSendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+using namespace frc;
+
+ADXL345_I2C::ADXL345_I2C(I2C::Port port, Range range, int deviceAddress)
+    : m_i2c(port, deviceAddress),
+      m_simDevice("Accel:ADXL345_I2C", port, deviceAddress) {
+  if (m_simDevice) {
+    m_simRange = m_simDevice.CreateEnumDouble("range", hal::SimDevice::kOutput,
+                                              {"2G", "4G", "8G", "16G"},
+                                              {2.0, 4.0, 8.0, 16.0}, 0);
+    m_simX = m_simDevice.CreateDouble("x", hal::SimDevice::kInput, 0.0);
+    m_simY = m_simDevice.CreateDouble("y", hal::SimDevice::kInput, 0.0);
+    m_simZ = m_simDevice.CreateDouble("z", hal::SimDevice::kInput, 0.0);
+  }
+  // Turn on the measurements
+  m_i2c.Write(kPowerCtlRegister, kPowerCtl_Measure);
+  // Specify the data format to read
+  SetRange(range);
+
+  HAL_ReportUsage(
+      fmt::format("I2C[{}][{}]", static_cast<int>(port), deviceAddress),
+      "ADXL345");
+
+  wpi::SendableRegistry::Add(this, "ADXL345_I2C", port);
+}
+
+I2C::Port ADXL345_I2C::GetI2CPort() const {
+  return m_i2c.GetPort();
+}
+
+int ADXL345_I2C::GetI2CDeviceAddress() const {
+  return m_i2c.GetDeviceAddress();
+}
+
+void ADXL345_I2C::SetRange(Range range) {
+  m_i2c.Write(kDataFormatRegister,
+              kDataFormat_FullRes | static_cast<uint8_t>(range));
+}
+
+double ADXL345_I2C::GetX() {
+  return GetAcceleration(kAxis_X);
+}
+
+double ADXL345_I2C::GetY() {
+  return GetAcceleration(kAxis_Y);
+}
+
+double ADXL345_I2C::GetZ() {
+  return GetAcceleration(kAxis_Z);
+}
+
+double ADXL345_I2C::GetAcceleration(ADXL345_I2C::Axes axis) {
+  if (axis == kAxis_X && m_simX) {
+    return m_simX.Get();
+  }
+  if (axis == kAxis_Y && m_simY) {
+    return m_simY.Get();
+  }
+  if (axis == kAxis_Z && m_simZ) {
+    return m_simZ.Get();
+  }
+  int16_t rawAccel = 0;
+  m_i2c.Read(kDataRegister + static_cast<int>(axis), sizeof(rawAccel),
+             reinterpret_cast<uint8_t*>(&rawAccel));
+  return rawAccel * kGsPerLSB;
+}
+
+ADXL345_I2C::AllAxes ADXL345_I2C::GetAccelerations() {
+  AllAxes data;
+  if (m_simX && m_simY && m_simZ) {
+    data.XAxis = m_simX.Get();
+    data.YAxis = m_simY.Get();
+    data.ZAxis = m_simZ.Get();
+    return data;
+  }
+  int16_t rawData[3];
+  m_i2c.Read(kDataRegister, sizeof(rawData),
+             reinterpret_cast<uint8_t*>(rawData));
+
+  data.XAxis = rawData[0] * kGsPerLSB;
+  data.YAxis = rawData[1] * kGsPerLSB;
+  data.ZAxis = rawData[2] * kGsPerLSB;
+  return data;
+}
+
+void ADXL345_I2C::InitSendable(nt::NTSendableBuilder& builder) {
+  builder.SetSmartDashboardType("3AxisAccelerometer");
+  builder.SetUpdateTable(
+      [this, x = nt::DoubleTopic{builder.GetTopic("X")}.Publish(),
+       y = nt::DoubleTopic{builder.GetTopic("Y")}.Publish(),
+       z = nt::DoubleTopic{builder.GetTopic("Z")}.Publish()]() mutable {
+        auto data = GetAccelerations();
+        x.Set(data.XAxis);
+        y.Set(data.YAxis);
+        z.Set(data.ZAxis);
+      });
+}

wpilibc/src/main/native/cpp/hardware/accelerometer/AnalogAccelerometer.cpp

@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/AnalogAccelerometer.h"
+
+#include <hal/UsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+AnalogAccelerometer::AnalogAccelerometer(int channel)
+    : AnalogAccelerometer(std::make_shared<AnalogInput>(channel)) {
+  wpi::SendableRegistry::AddChild(this, m_analogInput.get());
+}
+
+AnalogAccelerometer::AnalogAccelerometer(AnalogInput* channel)
+    : m_analogInput(channel, wpi::NullDeleter<AnalogInput>()) {
+  if (!channel) {
+    throw FRC_MakeError(err::NullParameter, "channel");
+  }
+  InitAccelerometer();
+}
+
+AnalogAccelerometer::AnalogAccelerometer(std::shared_ptr<AnalogInput> channel)
+    : m_analogInput(channel) {
+  if (!channel) {
+    throw FRC_MakeError(err::NullParameter, "channel");
+  }
+  InitAccelerometer();
+}
+
+double AnalogAccelerometer::GetAcceleration() const {
+  return (m_analogInput->GetVoltage() - m_zeroGVoltage) / m_voltsPerG;
+}
+
+void AnalogAccelerometer::SetSensitivity(double sensitivity) {
+  m_voltsPerG = sensitivity;
+}
+
+void AnalogAccelerometer::SetZero(double zero) {
+  m_zeroGVoltage = zero;
+}
+
+void AnalogAccelerometer::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Accelerometer");
+  builder.AddDoubleProperty(
+      "Value", [=, this] { return GetAcceleration(); }, nullptr);
+}
+
+void AnalogAccelerometer::InitAccelerometer() {
+  HAL_ReportUsage("IO", m_analogInput->GetChannel(), "Accelerometer");
+
+  wpi::SendableRegistry::Add(this, "Accelerometer",
+                             m_analogInput->GetChannel());
+}

wpilibc/src/main/native/cpp/hardware/bus/CAN.cpp

@@ -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.
+
+#include "frc/CAN.h"
+
+#include <utility>
+
+#include <hal/CAN.h>
+#include <hal/CANAPI.h>
+#include <hal/Errors.h>
+#include <hal/UsageReporting.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+CAN::CAN(int busId, int deviceId)
+    : CAN{busId, deviceId, kTeamManufacturer, kTeamDeviceType} {}
+
+CAN::CAN(int busId, int deviceId, int deviceManufacturer, int deviceType) {
+  int32_t status = 0;
+  m_handle = HAL_InitializeCAN(
+      busId, static_cast<HAL_CANManufacturer>(deviceManufacturer), deviceId,
+      static_cast<HAL_CANDeviceType>(deviceType), &status);
+  FRC_CheckErrorStatus(status, "device id {} mfg {} type {}", deviceId,
+                       deviceManufacturer, deviceType);
+
+  HAL_ReportUsage(
+      fmt::format("CAN[{}][{}][{}]", deviceType, deviceManufacturer, deviceId),
+      "");
+}
+
+void CAN::WritePacket(int apiId, const HAL_CANMessage& message) {
+  int32_t status = 0;
+  HAL_WriteCANPacket(m_handle, apiId, &message, &status);
+  FRC_CheckErrorStatus(status, "WritePacket");
+}
+
+void CAN::WritePacketRepeating(int apiId, const HAL_CANMessage& message,
+                               int repeatMs) {
+  int32_t status = 0;
+  HAL_WriteCANPacketRepeating(m_handle, apiId, &message, repeatMs, &status);
+  FRC_CheckErrorStatus(status, "WritePacketRepeating");
+}
+
+void CAN::WriteRTRFrame(int apiId, const HAL_CANMessage& message) {
+  int32_t status = 0;
+  HAL_WriteCANRTRFrame(m_handle, apiId, &message, &status);
+  FRC_CheckErrorStatus(status, "WriteRTRFrame");
+}
+
+int CAN::WritePacketNoError(int apiId, const HAL_CANMessage& message) {
+  int32_t status = 0;
+  HAL_WriteCANPacket(m_handle, apiId, &message, &status);
+  return status;
+}
+
+int CAN::WritePacketRepeatingNoError(int apiId, const HAL_CANMessage& message,
+                                     int repeatMs) {
+  int32_t status = 0;
+  HAL_WriteCANPacketRepeating(m_handle, apiId, &message, repeatMs, &status);
+  return status;
+}
+
+int CAN::WriteRTRFrameNoError(int apiId, const HAL_CANMessage& message) {
+  int32_t status = 0;
+  HAL_WriteCANRTRFrame(m_handle, apiId, &message, &status);
+  return status;
+}
+
+void CAN::StopPacketRepeating(int apiId) {
+  int32_t status = 0;
+  HAL_StopCANPacketRepeating(m_handle, apiId, &status);
+  FRC_CheckErrorStatus(status, "StopPacketRepeating");
+}
+
+bool CAN::ReadPacketNew(int apiId, HAL_CANReceiveMessage* data) {
+  int32_t status = 0;
+  HAL_ReadCANPacketNew(m_handle, apiId, data, &status);
+  if (status == HAL_ERR_CANSessionMux_MessageNotFound) {
+    return false;
+  }
+  if (status != 0) {
+    FRC_CheckErrorStatus(status, "ReadPacketNew");
+    return false;
+  } else {
+    return true;
+  }
+}
+
+bool CAN::ReadPacketLatest(int apiId, HAL_CANReceiveMessage* data) {
+  int32_t status = 0;
+  HAL_ReadCANPacketLatest(m_handle, apiId, data, &status);
+  if (status == HAL_ERR_CANSessionMux_MessageNotFound) {
+    return false;
+  }
+  if (status != 0) {
+    FRC_CheckErrorStatus(status, "ReadPacketLatest");
+    return false;
+  } else {
+    return true;
+  }
+}
+
+bool CAN::ReadPacketTimeout(int apiId, int timeoutMs,
+                            HAL_CANReceiveMessage* data) {
+  int32_t status = 0;
+  HAL_ReadCANPacketTimeout(m_handle, apiId, data, timeoutMs, &status);
+  if (status == HAL_CAN_TIMEOUT ||
+      status == HAL_ERR_CANSessionMux_MessageNotFound) {
+    return false;
+  }
+  if (status != 0) {
+    FRC_CheckErrorStatus(status, "ReadPacketTimeout");
+    return false;
+  } else {
+    return true;
+  }
+}

wpilibc/src/main/native/cpp/hardware/bus/I2C.cpp

@@ -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.
+
+#include "frc/I2C.h"
+
+#include <algorithm>
+
+#include <hal/I2C.h>
+#include <hal/UsageReporting.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+I2C::I2C(Port port, int deviceAddress)
+    : m_port(static_cast<HAL_I2CPort>(port)), m_deviceAddress(deviceAddress) {
+  int32_t status = 0;
+
+  HAL_InitializeI2C(m_port, &status);
+  FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(port));
+
+  HAL_ReportUsage(
+      fmt::format("I2C[{}][{}]", static_cast<int>(port), deviceAddress), "");
+}
+
+I2C::Port I2C::GetPort() const {
+  return static_cast<Port>(static_cast<int>(m_port));
+}
+
+int I2C::GetDeviceAddress() const {
+  return m_deviceAddress;
+}
+
+bool I2C::Transaction(uint8_t* dataToSend, int sendSize, uint8_t* dataReceived,
+                      int receiveSize) {
+  int32_t status = 0;
+  status = HAL_TransactionI2C(m_port, m_deviceAddress, dataToSend, sendSize,
+                              dataReceived, receiveSize);
+  return status < 0;
+}
+
+bool I2C::AddressOnly() {
+  return Transaction(nullptr, 0, nullptr, 0);
+}
+
+bool I2C::Write(int registerAddress, uint8_t data) {
+  uint8_t buffer[2];
+  buffer[0] = registerAddress;
+  buffer[1] = data;
+  int32_t status = 0;
+  status = HAL_WriteI2C(m_port, m_deviceAddress, buffer, sizeof(buffer));
+  return status < 0;
+}
+
+bool I2C::WriteBulk(uint8_t* data, int count) {
+  int32_t status = 0;
+  status = HAL_WriteI2C(m_port, m_deviceAddress, data, count);
+  return status < 0;
+}
+
+bool I2C::Read(int registerAddress, int count, uint8_t* buffer) {
+  if (count < 1) {
+    throw FRC_MakeError(err::ParameterOutOfRange, "count {}", count);
+  }
+  if (!buffer) {
+    throw FRC_MakeError(err::NullParameter, "buffer");
+  }
+  uint8_t regAddr = registerAddress;
+  return Transaction(&regAddr, sizeof(regAddr), buffer, count);
+}
+
+bool I2C::ReadOnly(int count, uint8_t* buffer) {
+  if (count < 1) {
+    throw FRC_MakeError(err::ParameterOutOfRange, "count {}", count);
+  }
+  if (!buffer) {
+    throw FRC_MakeError(err::NullParameter, "buffer");
+  }
+  return HAL_ReadI2C(m_port, m_deviceAddress, buffer, count) < 0;
+}
+
+bool I2C::VerifySensor(int registerAddress, int count,
+                       const uint8_t* expected) {
+  // TODO: Make use of all 7 read bytes
+  uint8_t deviceData[4];
+  for (int i = 0, curRegisterAddress = registerAddress; i < count;
+       i += 4, curRegisterAddress += 4) {
+    int toRead = std::min(count - i, 4);
+    // Read the chunk of data.  Return false if the sensor does not respond.
+    if (Read(curRegisterAddress, toRead, deviceData)) {
+      return false;
+    }
+
+    for (int j = 0; j < toRead; j++) {
+      if (deviceData[j] != expected[i + j]) {
+        return false;
+      }
+    }
+  }
+  return true;
+}

wpilibc/src/main/native/cpp/hardware/bus/SerialPort.cpp

@@ -0,0 +1,154 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/SerialPort.h"
+
+#include <string>
+
+#include <hal/SerialPort.h>
+#include <hal/UsageReporting.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+SerialPort::SerialPort(int baudRate, Port port, int dataBits,
+                       SerialPort::Parity parity,
+                       SerialPort::StopBits stopBits) {
+  int32_t status = 0;
+
+  m_portHandle =
+      HAL_InitializeSerialPort(static_cast<HAL_SerialPort>(port), &status);
+  FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(port));
+  HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
+  FRC_CheckErrorStatus(status, "SetSerialBaudRate {}", baudRate);
+  HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
+  FRC_CheckErrorStatus(status, "SetSerialDataBits {}", dataBits);
+  HAL_SetSerialParity(m_portHandle, parity, &status);
+  FRC_CheckErrorStatus(status, "SetSerialParity {}", static_cast<int>(parity));
+  HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
+  FRC_CheckErrorStatus(status, "SetSerialStopBits {}",
+                       static_cast<int>(stopBits));
+
+  // Set the default timeout to 5 seconds.
+  SetTimeout(5_s);
+
+  // Don't wait until the buffer is full to transmit.
+  SetWriteBufferMode(kFlushOnAccess);
+
+  DisableTermination();
+
+  HAL_ReportUsage("SerialPort", static_cast<int>(port), "");
+}
+
+SerialPort::SerialPort(int baudRate, std::string_view portName, Port port,
+                       int dataBits, SerialPort::Parity parity,
+                       SerialPort::StopBits stopBits) {
+  int32_t status = 0;
+
+  m_portHandle =
+      HAL_InitializeSerialPortDirect(static_cast<HAL_SerialPort>(port),
+                                     std::string(portName).c_str(), &status);
+  FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(port));
+  HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
+  FRC_CheckErrorStatus(status, "SetSerialBaudRate {}", baudRate);
+  HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
+  FRC_CheckErrorStatus(status, "SetSerialDataBits {}", dataBits);
+  HAL_SetSerialParity(m_portHandle, parity, &status);
+  FRC_CheckErrorStatus(status, "SetSerialParity {}", static_cast<int>(parity));
+  HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
+  FRC_CheckErrorStatus(status, "SetSerialStopBits {}",
+                       static_cast<int>(stopBits));
+
+  // Set the default timeout to 5 seconds.
+  SetTimeout(5_s);
+
+  // Don't wait until the buffer is full to transmit.
+  SetWriteBufferMode(kFlushOnAccess);
+
+  DisableTermination();
+
+  HAL_ReportUsage("SerialPort", static_cast<int>(port), "");
+}
+
+void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl) {
+  int32_t status = 0;
+  HAL_SetSerialFlowControl(m_portHandle, flowControl, &status);
+  FRC_CheckErrorStatus(status, "SetFlowControl {}",
+                       static_cast<int>(flowControl));
+}
+
+void SerialPort::EnableTermination(char terminator) {
+  int32_t status = 0;
+  HAL_EnableSerialTermination(m_portHandle, terminator, &status);
+  FRC_CheckErrorStatus(status, "EnableTermination {}", terminator);
+}
+
+void SerialPort::DisableTermination() {
+  int32_t status = 0;
+  HAL_DisableSerialTermination(m_portHandle, &status);
+  FRC_CheckErrorStatus(status, "DisableTermination");
+}
+
+int SerialPort::GetBytesReceived() {
+  int32_t status = 0;
+  int retVal = HAL_GetSerialBytesReceived(m_portHandle, &status);
+  FRC_CheckErrorStatus(status, "GetBytesReceived");
+  return retVal;
+}
+
+int SerialPort::Read(char* buffer, int count) {
+  int32_t status = 0;
+  int retVal = HAL_ReadSerial(m_portHandle, buffer, count, &status);
+  FRC_CheckErrorStatus(status, "Read");
+  return retVal;
+}
+
+int SerialPort::Write(const char* buffer, int count) {
+  return Write(std::string_view(buffer, static_cast<size_t>(count)));
+}
+
+int SerialPort::Write(std::string_view buffer) {
+  int32_t status = 0;
+  int retVal =
+      HAL_WriteSerial(m_portHandle, buffer.data(), buffer.size(), &status);
+  FRC_CheckErrorStatus(status, "Write");
+  return retVal;
+}
+
+void SerialPort::SetTimeout(units::second_t timeout) {
+  int32_t status = 0;
+  HAL_SetSerialTimeout(m_portHandle, timeout.value(), &status);
+  FRC_CheckErrorStatus(status, "SetTimeout");
+}
+
+void SerialPort::SetReadBufferSize(int size) {
+  int32_t status = 0;
+  HAL_SetSerialReadBufferSize(m_portHandle, size, &status);
+  FRC_CheckErrorStatus(status, "SetReadBufferSize {}", size);
+}
+
+void SerialPort::SetWriteBufferSize(int size) {
+  int32_t status = 0;
+  HAL_SetSerialWriteBufferSize(m_portHandle, size, &status);
+  FRC_CheckErrorStatus(status, "SetWriteBufferSize {}", size);
+}
+
+void SerialPort::SetWriteBufferMode(SerialPort::WriteBufferMode mode) {
+  int32_t status = 0;
+  HAL_SetSerialWriteMode(m_portHandle, mode, &status);
+  FRC_CheckErrorStatus(status, "SetWriteBufferMode {}", static_cast<int>(mode));
+}
+
+void SerialPort::Flush() {
+  int32_t status = 0;
+  HAL_FlushSerial(m_portHandle, &status);
+  FRC_CheckErrorStatus(status, "Flush");
+}
+
+void SerialPort::Reset() {
+  int32_t status = 0;
+  HAL_ClearSerial(m_portHandle, &status);
+  FRC_CheckErrorStatus(status, "Reset");
+}

wpilibc/src/main/native/cpp/hardware/discrete/AnalogInput.cpp

@@ -0,0 +1,132 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/AnalogInput.h"
+
+#include <string>
+
+#include <hal/AnalogInput.h>
+#include <hal/HALBase.h>
+#include <hal/Ports.h>
+#include <hal/UsageReporting.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+#include "frc/Timer.h"
+
+using namespace frc;
+
+AnalogInput::AnalogInput(int channel) {
+  if (!SensorUtil::CheckAnalogInputChannel(channel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel);
+  }
+
+  m_channel = channel;
+  int32_t status = 0;
+  std::string stackTrace = wpi::GetStackTrace(1);
+  m_port = HAL_InitializeAnalogInputPort(channel, stackTrace.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
+
+  HAL_ReportUsage("IO", channel, "AnalogInput");
+
+  wpi::SendableRegistry::Add(this, "AnalogInput", channel);
+}
+
+int AnalogInput::GetValue() const {
+  int32_t status = 0;
+  int value = HAL_GetAnalogValue(m_port, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+  return value;
+}
+
+int AnalogInput::GetAverageValue() const {
+  int32_t status = 0;
+  int value = HAL_GetAnalogAverageValue(m_port, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+  return value;
+}
+
+double AnalogInput::GetVoltage() const {
+  int32_t status = 0;
+  double voltage = HAL_GetAnalogVoltage(m_port, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+  return voltage;
+}
+
+double AnalogInput::GetAverageVoltage() const {
+  int32_t status = 0;
+  double voltage = HAL_GetAnalogAverageVoltage(m_port, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+  return voltage;
+}
+
+int AnalogInput::GetChannel() const {
+  return m_channel;
+}
+
+void AnalogInput::SetAverageBits(int bits) {
+  int32_t status = 0;
+  HAL_SetAnalogAverageBits(m_port, bits, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+}
+
+int AnalogInput::GetAverageBits() const {
+  int32_t status = 0;
+  int averageBits = HAL_GetAnalogAverageBits(m_port, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+  return averageBits;
+}
+
+void AnalogInput::SetOversampleBits(int bits) {
+  int32_t status = 0;
+  HAL_SetAnalogOversampleBits(m_port, bits, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+}
+
+int AnalogInput::GetOversampleBits() const {
+  int32_t status = 0;
+  int oversampleBits = HAL_GetAnalogOversampleBits(m_port, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+  return oversampleBits;
+}
+
+int AnalogInput::GetLSBWeight() const {
+  int32_t status = 0;
+  int lsbWeight = HAL_GetAnalogLSBWeight(m_port, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+  return lsbWeight;
+}
+
+int AnalogInput::GetOffset() const {
+  int32_t status = 0;
+  int offset = HAL_GetAnalogOffset(m_port, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+  return offset;
+}
+
+void AnalogInput::SetSampleRate(double samplesPerSecond) {
+  int32_t status = 0;
+  HAL_SetAnalogSampleRate(samplesPerSecond, &status);
+  FRC_CheckErrorStatus(status, "SetSampleRate");
+}
+
+double AnalogInput::GetSampleRate() {
+  int32_t status = 0;
+  double sampleRate = HAL_GetAnalogSampleRate(&status);
+  FRC_CheckErrorStatus(status, "GetSampleRate");
+  return sampleRate;
+}
+
+void AnalogInput::SetSimDevice(HAL_SimDeviceHandle device) {
+  HAL_SetAnalogInputSimDevice(m_port, device);
+}
+
+void AnalogInput::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Analog Input");
+  builder.AddDoubleProperty(
+      "Value", [=, this] { return GetAverageVoltage(); }, nullptr);
+}

wpilibc/src/main/native/cpp/hardware/discrete/DigitalInput.cpp

@@ -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.
+
+#include "frc/DigitalInput.h"
+
+#include <string>
+
+#include <hal/DIO.h>
+#include <hal/HALBase.h>
+#include <hal/Ports.h>
+#include <hal/UsageReporting.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+
+DigitalInput::DigitalInput(int channel) {
+  if (!SensorUtil::CheckDigitalChannel(channel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel);
+  }
+  m_channel = channel;
+
+  int32_t status = 0;
+  std::string stackTrace = wpi::GetStackTrace(1);
+  m_handle = HAL_InitializeDIOPort(channel, true, stackTrace.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
+
+  HAL_ReportUsage("IO", channel, "DigitalInput");
+  wpi::SendableRegistry::Add(this, "DigitalInput", channel);
+}
+
+bool DigitalInput::Get() const {
+  int32_t status = 0;
+  bool value = HAL_GetDIO(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+  return value;
+}
+
+void DigitalInput::SetSimDevice(HAL_SimDeviceHandle device) {
+  HAL_SetDIOSimDevice(m_handle, device);
+}
+
+int DigitalInput::GetChannel() const {
+  return m_channel;
+}
+
+void DigitalInput::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Digital Input");
+  builder.AddBooleanProperty("Value", [=, this] { return Get(); }, nullptr);
+}

wpilibc/src/main/native/cpp/hardware/discrete/DigitalOutput.cpp

@@ -0,0 +1,151 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/DigitalOutput.h"
+
+#include <string>
+
+#include <hal/DIO.h>
+#include <hal/HALBase.h>
+#include <hal/Ports.h>
+#include <hal/UsageReporting.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+
+DigitalOutput::DigitalOutput(int channel) {
+  m_pwmGenerator = HAL_kInvalidHandle;
+  if (!SensorUtil::CheckDigitalChannel(channel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel);
+  }
+  m_channel = channel;
+
+  int32_t status = 0;
+  std::string stackTrace = wpi::GetStackTrace(1);
+  m_handle = HAL_InitializeDIOPort(channel, false, stackTrace.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
+
+  HAL_ReportUsage("IO", channel, "DigitalOutput");
+  wpi::SendableRegistry::Add(this, "DigitalOutput", channel);
+}
+
+DigitalOutput::~DigitalOutput() {
+  if (m_handle != HAL_kInvalidHandle) {
+    // Disable the PWM in case it was running.
+    try {
+      DisablePWM();
+    } catch (const RuntimeError& e) {
+      e.Report();
+    }
+  }
+}
+
+void DigitalOutput::Set(bool value) {
+  int32_t status = 0;
+  HAL_SetDIO(m_handle, value, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+}
+
+bool DigitalOutput::Get() const {
+  int32_t status = 0;
+  bool val = HAL_GetDIO(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+  return val;
+}
+
+int DigitalOutput::GetChannel() const {
+  return m_channel;
+}
+
+void DigitalOutput::Pulse(units::second_t pulseLength) {
+  int32_t status = 0;
+  HAL_Pulse(m_handle, pulseLength.value(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+}
+
+bool DigitalOutput::IsPulsing() const {
+  int32_t status = 0;
+  bool value = HAL_IsPulsing(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+  return value;
+}
+
+void DigitalOutput::SetPWMRate(double rate) {
+  int32_t status = 0;
+  HAL_SetDigitalPWMRate(rate, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+}
+
+void DigitalOutput::EnablePPS(double dutyCycle) {
+  if (m_pwmGenerator != HAL_kInvalidHandle) {
+    return;
+  }
+
+  int32_t status = 0;
+
+  m_pwmGenerator = HAL_AllocateDigitalPWM(&status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+
+  HAL_SetDigitalPWMPPS(m_pwmGenerator, dutyCycle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+
+  HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, m_channel, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+}
+
+void DigitalOutput::EnablePWM(double initialDutyCycle) {
+  if (m_pwmGenerator != HAL_kInvalidHandle) {
+    return;
+  }
+
+  int32_t status = 0;
+
+  m_pwmGenerator = HAL_AllocateDigitalPWM(&status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+
+  HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, initialDutyCycle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+
+  HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, m_channel, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+}
+
+void DigitalOutput::DisablePWM() {
+  if (m_pwmGenerator == HAL_kInvalidHandle) {
+    return;
+  }
+
+  int32_t status = 0;
+
+  // Disable the output by routing to a dead bit.
+  HAL_SetDigitalPWMOutputChannel(m_pwmGenerator,
+                                 SensorUtil::GetNumDigitalChannels(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+
+  HAL_FreeDigitalPWM(m_pwmGenerator);
+
+  m_pwmGenerator = HAL_kInvalidHandle;
+}
+
+void DigitalOutput::UpdateDutyCycle(double dutyCycle) {
+  int32_t status = 0;
+  HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, dutyCycle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+}
+
+void DigitalOutput::SetSimDevice(HAL_SimDeviceHandle device) {
+  HAL_SetDIOSimDevice(m_handle, device);
+}
+
+void DigitalOutput::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Digital Output");
+  builder.AddBooleanProperty(
+      "Value", [=, this] { return Get(); },
+      [=, this](bool value) { Set(value); });
+}

wpilibc/src/main/native/cpp/hardware/discrete/PWM.cpp

@@ -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.
+
+#include "frc/PWM.h"
+
+#include <utility>
+
+#include <hal/HALBase.h>
+#include <hal/PWM.h>
+#include <hal/Ports.h>
+#include <hal/UsageReporting.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+
+PWM::PWM(int channel, bool registerSendable) {
+  if (!SensorUtil::CheckPWMChannel(channel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel);
+  }
+
+  auto stack = wpi::GetStackTrace(1);
+  int32_t status = 0;
+  m_handle = HAL_InitializePWMPort(channel, stack.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
+
+  m_channel = channel;
+
+  SetDisabled();
+
+  HAL_ReportUsage("IO", channel, "PWM");
+  if (registerSendable) {
+    wpi::SendableRegistry::Add(this, "PWM", channel);
+  }
+}
+
+PWM::~PWM() {
+  if (m_handle != HAL_kInvalidHandle) {
+    SetDisabled();
+  }
+}
+
+void PWM::SetPulseTime(units::microsecond_t time) {
+  int32_t status = 0;
+  HAL_SetPWMPulseTimeMicroseconds(m_handle, time.value(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+}
+
+units::microsecond_t PWM::GetPulseTime() const {
+  int32_t status = 0;
+  double value = HAL_GetPWMPulseTimeMicroseconds(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+
+  return units::microsecond_t{value};
+}
+
+void PWM::SetDisabled() {
+  int32_t status = 0;
+  HAL_SetPWMPulseTimeMicroseconds(m_handle, 0, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+}
+
+void PWM::SetOutputPeriod(OutputPeriod mult) {
+  int32_t status = 0;
+
+  switch (mult) {
+    case kOutputPeriod_20Ms:
+      HAL_SetPWMOutputPeriod(m_handle, 3,
+                             &status);  // Squelch 3 out of 4 outputs
+      break;
+    case kOutputPeriod_10Ms:
+      HAL_SetPWMOutputPeriod(m_handle, 1,
+                             &status);  // Squelch 1 out of 2 outputs
+      break;
+    case kOutputPeriod_5Ms:
+      HAL_SetPWMOutputPeriod(m_handle, 0,
+                             &status);  // Don't squelch any outputs
+      break;
+    default:
+      throw FRC_MakeError(err::InvalidParameter, "OutputPeriod value {}",
+                          static_cast<int>(mult));
+  }
+
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+}
+
+int PWM::GetChannel() const {
+  return m_channel;
+}
+
+void PWM::SetSimDevice(HAL_SimDeviceHandle device) {
+  HAL_SetPWMSimDevice(m_handle, device);
+}
+
+void PWM::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("PWM");
+  builder.SetActuator(true);
+  builder.AddDoubleProperty(
+      "Value", [=, this] { return GetPulseTime().value(); },
+      [=, this](double value) { SetPulseTime(units::millisecond_t{value}); });
+}

wpilibc/src/main/native/cpp/hardware/imu/OnboardIMU.cpp

@@ -0,0 +1,161 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/OnboardIMU.h"
+
+#include <hal/IMU.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+OnboardIMU::OnboardIMU(MountOrientation mountOrientation)
+    : m_mountOrientation{mountOrientation} {
+  // TODO: usage reporting
+}
+
+units::radian_t OnboardIMU::GetYawNoOffset() {
+  int64_t timestamp;
+  double val;
+  switch (m_mountOrientation) {
+    case kFlat:
+      val = HAL_GetIMUYawFlat(&timestamp);
+      break;
+    case kLandscape:
+      val = HAL_GetIMUYawLandscape(&timestamp);
+      break;
+    case kPortrait:
+      val = HAL_GetIMUYawPortrait(&timestamp);
+      break;
+    default:
+      val = 0;
+  }
+  return units::radian_t{val};
+}
+
+units::radian_t OnboardIMU::GetYaw() {
+  return GetYawNoOffset() - m_yawOffset;
+}
+
+void OnboardIMU::ResetYaw() {
+  m_yawOffset = GetYawNoOffset();
+}
+
+Rotation2d OnboardIMU::GetRotation2d() {
+  return Rotation2d{GetYaw()};
+}
+
+Rotation3d OnboardIMU::GetRotation3d() {
+  return Rotation3d{GetQuaternion()};
+}
+
+Quaternion OnboardIMU::GetQuaternion() {
+  HAL_Quaternion val;
+  int32_t status = 0;
+  HAL_GetIMUQuaternion(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return Quaternion{val.w, val.x, val.y, val.z};
+}
+
+units::radian_t OnboardIMU::GetAngleX() {
+  HAL_EulerAngles3d val;
+  int32_t status = 0;
+  switch (m_mountOrientation) {
+    case kFlat:
+      HAL_GetIMUEulerAnglesFlat(&val, &status);
+      break;
+    case kLandscape:
+      HAL_GetIMUEulerAnglesLandscape(&val, &status);
+      break;
+    case kPortrait:
+      HAL_GetIMUEulerAnglesPortrait(&val, &status);
+      break;
+  }
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radian_t{val.x};
+}
+
+units::radian_t OnboardIMU::GetAngleY() {
+  HAL_EulerAngles3d val;
+  int32_t status = 0;
+  switch (m_mountOrientation) {
+    case kFlat:
+      HAL_GetIMUEulerAnglesFlat(&val, &status);
+      break;
+    case kLandscape:
+      HAL_GetIMUEulerAnglesLandscape(&val, &status);
+      break;
+    case kPortrait:
+      HAL_GetIMUEulerAnglesPortrait(&val, &status);
+      break;
+  }
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radian_t{val.y};
+}
+
+units::radian_t OnboardIMU::GetAngleZ() {
+  HAL_EulerAngles3d val;
+  int32_t status = 0;
+  switch (m_mountOrientation) {
+    case kFlat:
+      HAL_GetIMUEulerAnglesFlat(&val, &status);
+      break;
+    case kLandscape:
+      HAL_GetIMUEulerAnglesLandscape(&val, &status);
+      break;
+    case kPortrait:
+      HAL_GetIMUEulerAnglesPortrait(&val, &status);
+      break;
+  }
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radian_t{val.z};
+}
+
+units::radians_per_second_t OnboardIMU::GetGyroRateX() {
+  HAL_GyroRate3d val;
+  int32_t status = 0;
+  HAL_GetIMUGyroRates(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radians_per_second_t{val.x};
+}
+
+units::radians_per_second_t OnboardIMU::GetGyroRateY() {
+  HAL_GyroRate3d val;
+  int32_t status = 0;
+  HAL_GetIMUGyroRates(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radians_per_second_t{val.y};
+}
+
+units::radians_per_second_t OnboardIMU::GetGyroRateZ() {
+  HAL_GyroRate3d val;
+  int32_t status = 0;
+  HAL_GetIMUGyroRates(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radians_per_second_t{val.z};
+}
+
+units::meters_per_second_squared_t OnboardIMU::GetAccelX() {
+  HAL_Acceleration3d val;
+  int32_t status = 0;
+  HAL_GetIMUAcceleration(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::meters_per_second_squared_t{val.x};
+}
+
+units::meters_per_second_squared_t OnboardIMU::GetAccelY() {
+  HAL_Acceleration3d val;
+  int32_t status = 0;
+  HAL_GetIMUAcceleration(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::meters_per_second_squared_t{val.x};
+}
+
+units::meters_per_second_squared_t OnboardIMU::GetAccelZ() {
+  HAL_Acceleration3d val;
+  int32_t status = 0;
+  HAL_GetIMUAcceleration(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::meters_per_second_squared_t{val.x};
+}

wpilibc/src/main/native/cpp/hardware/led/AddressableLED.cpp

@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/AddressableLED.h"
+
+#include <algorithm>
+
+#include <hal/AddressableLED.h>
+#include <hal/HALBase.h>
+#include <hal/PWM.h>
+#include <hal/Ports.h>
+#include <hal/UsageReporting.h>
+#include <wpi/StackTrace.h>
+
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+
+AddressableLED::AddressableLED(int channel) : m_channel{channel} {
+  if (!SensorUtil::CheckDigitalChannel(channel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel);
+  }
+
+  int32_t status = 0;
+  auto stack = wpi::GetStackTrace(1);
+  m_handle = HAL_InitializeAddressableLED(channel, stack.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
+
+  HAL_ReportUsage("IO", channel, "AddressableLED");
+}
+
+void AddressableLED::SetColorOrder(AddressableLED::ColorOrder order) {
+  m_colorOrder = order;
+}
+
+void AddressableLED::SetStart(int start) {
+  m_start = start;
+  int32_t status = 0;
+  HAL_SetAddressableLEDStart(m_handle, start, &status);
+  FRC_CheckErrorStatus(status, "Channel {} start {}", m_channel, start);
+}
+
+void AddressableLED::SetLength(int length) {
+  m_length = length;
+  int32_t status = 0;
+  HAL_SetAddressableLEDLength(m_handle, length, &status);
+  FRC_CheckErrorStatus(status, "Channel {} length {}", m_channel, length);
+}
+
+static_assert(sizeof(AddressableLED::LEDData) == sizeof(HAL_AddressableLEDData),
+              "LED Structs MUST be the same size");
+
+void AddressableLED::SetData(std::span<const LEDData> ledData) {
+  int32_t status = 0;
+  HAL_SetAddressableLEDData(
+      m_start, std::min(static_cast<size_t>(m_length), ledData.size()),
+      static_cast<HAL_AddressableLEDColorOrder>(m_colorOrder), ledData.data(),
+      &status);
+  FRC_CheckErrorStatus(status, "Port {}", m_channel);
+}
+
+void AddressableLED::SetData(std::initializer_list<LEDData> ledData) {
+  SetData(std::span{ledData.begin(), ledData.end()});
+}
+
+void AddressableLED::SetGlobalData(int start, ColorOrder colorOrder,
+                                   std::span<const LEDData> ledData) {
+  int32_t status = 0;
+  HAL_SetAddressableLEDData(
+      start, ledData.size(),
+      static_cast<HAL_AddressableLEDColorOrder>(colorOrder), ledData.data(),
+      &status);
+  FRC_CheckErrorStatus(status, "");
+}
+
+void AddressableLED::LEDData::SetHSV(int h, int s, int v) {
+  SetLED(Color::FromHSV(h, s, v));
+}

wpilibc/src/main/native/cpp/hardware/led/LEDPattern.cpp

@@ -0,0 +1,324 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/LEDPattern.h"
+
+#include <algorithm>
+#include <cmath>
+#include <numbers>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include <wpi/MathExtras.h>
+#include <wpi/timestamp.h>
+
+#include "frc/MathUtil.h"
+
+using namespace frc;
+
+LEDPattern::LEDPattern(std::function<void(frc::LEDPattern::LEDReader,
+                                          std::function<void(int, frc::Color)>)>
+                           impl)
+    : m_impl(std::move(impl)) {}
+
+void LEDPattern::ApplyTo(LEDPattern::LEDReader reader,
+                         std::function<void(int, frc::Color)> writer) const {
+  m_impl(reader, writer);
+}
+
+void LEDPattern::ApplyTo(std::span<AddressableLED::LEDData> data,
+                         std::function<void(int, frc::Color)> writer) const {
+  ApplyTo(LEDPattern::LEDReader{[=](size_t i) { return data[i]; }, data.size()},
+          writer);
+}
+
+void LEDPattern::ApplyTo(std::span<AddressableLED::LEDData> data) const {
+  ApplyTo(data, [&](int index, Color color) { data[index].SetLED(color); });
+}
+
+LEDPattern LEDPattern::MapIndex(
+    std::function<size_t(size_t, size_t)> indexMapper) {
+  return LEDPattern{[self = *this, indexMapper](auto data, auto writer) {
+    size_t bufLen = data.size();
+    self.ApplyTo(
+        LEDPattern::LEDReader{
+            [=](auto i) { return data[indexMapper(bufLen, i)]; }, bufLen},
+        [&](int i, Color color) { writer(indexMapper(bufLen, i), color); });
+  }};
+}
+
+LEDPattern LEDPattern::Reversed() {
+  return MapIndex([](size_t bufLen, size_t i) { return bufLen - 1 - i; });
+}
+
+LEDPattern LEDPattern::OffsetBy(int offset) {
+  return MapIndex([offset](size_t bufLen, size_t i) {
+    return frc::FloorMod(static_cast<int>(i) + offset,
+                         static_cast<int>(bufLen));
+  });
+}
+
+LEDPattern LEDPattern::ScrollAtRelativeSpeed(units::hertz_t velocity) {
+  // velocity is in terms of LED lengths per second (1_hz = full cycle per
+  // second, 0.5_hz = half cycle per second, 2_hz = two cycles per second)
+  // Invert and multiply by 1,000,000 to get microseconds
+  double periodMicros = 1e6 / velocity.value();
+
+  return MapIndex([=](size_t bufLen, size_t i) {
+    auto now = wpi::Now();
+
+    // index should move by (bufLen) / (period)
+    double t =
+        (now % static_cast<int64_t>(std::floor(periodMicros))) / periodMicros;
+    int offset = static_cast<int>(std::floor(t * bufLen));
+
+    return frc::FloorMod(static_cast<int>(i) + offset,
+                         static_cast<int>(bufLen));
+  });
+}
+
+LEDPattern LEDPattern::ScrollAtAbsoluteSpeed(
+    units::meters_per_second_t velocity, units::meter_t ledSpacing) {
+  // Velocity is in terms of meters per second
+  // Multiply by 1,000,000 to use microseconds instead of seconds
+  auto microsPerLed =
+      static_cast<int64_t>(std::floor((ledSpacing / velocity).value() * 1e6));
+
+  return MapIndex([=](size_t bufLen, size_t i) {
+    auto now = wpi::Now();
+
+    // every step in time that's a multiple of microsPerLED will increment
+    // the offset by 1
+    // cast unsigned int64 `now` to a signed int64 so we can get negative
+    // offset values for negative velocities
+    auto offset = static_cast<int64_t>(now) / microsPerLed;
+
+    return frc::FloorMod(static_cast<int>(i) + offset,
+                         static_cast<int>(bufLen));
+  });
+}
+
+LEDPattern LEDPattern::Blink(units::second_t onTime, units::second_t offTime) {
+  auto totalMicros = units::microsecond_t{onTime + offTime}.to<uint64_t>();
+  auto onMicros = units::microsecond_t{onTime}.to<uint64_t>();
+
+  return LEDPattern{[=, self = *this](auto data, auto writer) {
+    if (wpi::Now() % totalMicros < onMicros) {
+      self.ApplyTo(data, writer);
+    } else {
+      LEDPattern::Off().ApplyTo(data, writer);
+    }
+  }};
+}
+
+LEDPattern LEDPattern::Blink(units::second_t onTime) {
+  return LEDPattern::Blink(onTime, onTime);
+}
+
+LEDPattern LEDPattern::SynchronizedBlink(std::function<bool()> signal) {
+  return LEDPattern{[=, self = *this](auto data, auto writer) {
+    if (signal()) {
+      self.ApplyTo(data, writer);
+    } else {
+      LEDPattern::Off().ApplyTo(data, writer);
+    }
+  }};
+}
+
+LEDPattern LEDPattern::Breathe(units::second_t period) {
+  auto periodMicros = units::microsecond_t{period};
+
+  return LEDPattern{[periodMicros, self = *this](auto data, auto writer) {
+    self.ApplyTo(data, [&writer, periodMicros](int i, Color color) {
+      double t = (wpi::Now() % periodMicros.to<uint64_t>()) /
+                 periodMicros.to<double>();
+      double phase = t * 2 * std::numbers::pi;
+
+      // Apply the cosine function and shift its output from [-1, 1] to [0, 1]
+      // Use cosine so the period starts at 100% brightness
+      double dim = (std::cos(phase) + 1) / 2.0;
+
+      writer(i, Color{color.red * dim, color.green * dim, color.blue * dim});
+    });
+  }};
+}
+
+LEDPattern LEDPattern::OverlayOn(const LEDPattern& base) {
+  return LEDPattern{[self = *this, base](auto data, auto writer) {
+    // write the base pattern down first...
+    base.ApplyTo(data, writer);
+
+    // ... then, overwrite with illuminated LEDs from the overlay
+    self.ApplyTo(data, [&](int i, Color color) {
+      if (color.red > 0 || color.green > 0 || color.blue > 0) {
+        writer(i, color);
+      }
+    });
+  }};
+}
+
+LEDPattern LEDPattern::Blend(const LEDPattern& other) {
+  return LEDPattern{[self = *this, other](auto data, auto writer) {
+    // Apply the current pattern down as normal...
+    self.ApplyTo(data, writer);
+
+    other.ApplyTo(data, [&](int i, Color color) {
+      // ... then read the result and average it with the output from the other
+      // pattern
+      writer(i, Color{(data[i].r / 255.0 + color.red) / 2,
+                      (data[i].g / 255.0 + color.green) / 2,
+                      (data[i].b / 255.0 + color.blue) / 2});
+    });
+  }};
+}
+
+LEDPattern LEDPattern::Mask(const LEDPattern& mask) {
+  return LEDPattern{[self = *this, mask](auto data, auto writer) {
+    // Apply the current pattern down as normal...
+    self.ApplyTo(data, writer);
+
+    mask.ApplyTo(data, [&](int i, Color color) {
+      auto currentColor = data[i];
+      // ... then perform a bitwise AND operation on each channel to apply the
+      // mask
+      writer(i, Color{currentColor.r & static_cast<uint8_t>(255 * color.red),
+                      currentColor.g & static_cast<uint8_t>(255 * color.green),
+                      currentColor.b & static_cast<uint8_t>(255 * color.blue)});
+    });
+  }};
+}
+
+LEDPattern LEDPattern::AtBrightness(double relativeBrightness) {
+  return LEDPattern{[relativeBrightness, self = *this](auto data, auto writer) {
+    self.ApplyTo(data, [&](int i, Color color) {
+      writer(i, Color{color.red * relativeBrightness,
+                      color.green * relativeBrightness,
+                      color.blue * relativeBrightness});
+    });
+  }};
+}
+
+// Static constants and functions
+
+LEDPattern LEDPattern::Off() {
+  return LEDPattern::Solid(Color::kBlack);
+}
+
+LEDPattern LEDPattern::Solid(const Color color) {
+  return LEDPattern{[=](auto data, auto writer) {
+    auto bufLen = data.size();
+    for (size_t i = 0; i < bufLen; i++) {
+      writer(i, color);
+    }
+  }};
+}
+
+LEDPattern LEDPattern::ProgressMaskLayer(
+    std::function<double()> progressFunction) {
+  return LEDPattern{[=](auto data, auto writer) {
+    double progress = std::clamp(progressFunction(), 0.0, 1.0);
+    auto bufLen = data.size();
+    size_t max = bufLen * progress;
+
+    for (size_t led = 0; led < max; led++) {
+      writer(led, Color::kWhite);
+    }
+    for (size_t led = max; led < bufLen; led++) {
+      writer(led, Color::kBlack);
+    }
+  }};
+}
+
+LEDPattern LEDPattern::Steps(std::span<const std::pair<double, Color>> steps) {
+  if (steps.size() == 0) {
+    // no colors specified
+    return LEDPattern::Off();
+  }
+  if (steps.size() == 1 && steps[0].first == 0) {
+    // only one color specified, just show a static color
+    return LEDPattern::Solid(steps[0].second);
+  }
+
+  return LEDPattern{[steps = std::vector(steps.begin(), steps.end())](
+                        auto data, auto writer) {
+    auto bufLen = data.size();
+
+    // precompute relevant positions for this buffer so we don't need to do a
+    // check on every single LED index
+    std::unordered_map<int, Color> stopPositions;
+
+    for (auto step : steps) {
+      stopPositions[std::floor(step.first * bufLen)] = step.second;
+    }
+    auto currentColor = Color::kBlack;
+    for (size_t led = 0; led < bufLen; led++) {
+      if (stopPositions.contains(led)) {
+        currentColor = stopPositions[led];
+      }
+      writer(led, currentColor);
+    }
+  }};
+}
+
+LEDPattern LEDPattern::Steps(
+    std::initializer_list<std::pair<double, Color>> steps) {
+  return Steps(std::span{steps.begin(), steps.end()});
+}
+
+LEDPattern LEDPattern::Gradient(GradientType type,
+                                std::span<const Color> colors) {
+  if (colors.size() == 0) {
+    // no colors specified
+    return LEDPattern::Off();
+  }
+  if (colors.size() == 1) {
+    // only one color specified, just show a static color
+    return LEDPattern::Solid(colors[0]);
+  }
+
+  return LEDPattern{[type, colors = std::vector(colors.begin(), colors.end())](
+                        auto data, auto writer) {
+    size_t numSegments = colors.size();
+    auto bufLen = data.size();
+    int ledsPerSegment = 0;
+    switch (type) {
+      case kContinuous:
+        ledsPerSegment = bufLen / numSegments;
+        break;
+      case kDiscontinuous:
+        ledsPerSegment = (bufLen - 1) / (numSegments - 1);
+        break;
+    }
+
+    for (size_t led = 0; led < bufLen; led++) {
+      int colorIndex = (led / ledsPerSegment) % numSegments;
+      int nextColorIndex = (colorIndex + 1) % numSegments;
+      double t = std::fmod(led / static_cast<double>(ledsPerSegment), 1.0);
+
+      auto color = colors[colorIndex];
+      auto nextColor = colors[nextColorIndex];
+
+      Color gradientColor{wpi::Lerp(color.red, nextColor.red, t),
+                          wpi::Lerp(color.green, nextColor.green, t),
+                          wpi::Lerp(color.blue, nextColor.blue, t)};
+      writer(led, gradientColor);
+    }
+  }};
+}
+
+LEDPattern LEDPattern::Gradient(GradientType type,
+                                std::initializer_list<Color> colors) {
+  return Gradient(type, std::span{colors.begin(), colors.end()});
+}
+
+LEDPattern LEDPattern::Rainbow(int saturation, int value) {
+  return LEDPattern{[=](auto data, auto writer) {
+    auto bufLen = data.size();
+    for (size_t led = 0; led < bufLen; led++) {
+      int hue = ((led * 180) / bufLen) % 180;
+      writer(led, Color::FromHSV(hue, saturation, value));
+    }
+  }};
+}

wpilibc/src/main/native/cpp/hardware/motor/MotorSafety.cpp

@@ -0,0 +1,184 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/MotorSafety.h"
+
+#include <algorithm>
+#include <utility>
+
+#include <hal/DriverStation.h>
+#include <wpi/SafeThread.h>
+#include <wpi/SmallPtrSet.h>
+
+#include "frc/DriverStation.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+namespace {
+class Thread : public wpi::SafeThread {
+ public:
+  Thread() {}
+  void Main() override;
+};
+
+void Thread::Main() {
+  wpi::Event event{false, false};
+  HAL_ProvideNewDataEventHandle(event.GetHandle());
+
+  int safetyCounter = 0;
+  while (m_active) {
+    bool timedOut = false;
+    bool signaled = wpi::WaitForObject(event.GetHandle(), 0.1, &timedOut);
+    if (signaled) {
+      HAL_ControlWord controlWord;
+      std::memset(&controlWord, 0, sizeof(controlWord));
+      HAL_GetControlWord(&controlWord);
+      if (!(controlWord.enabled && controlWord.dsAttached)) {
+        safetyCounter = 0;
+      }
+      if (++safetyCounter >= 4) {
+        MotorSafety::CheckMotors();
+        safetyCounter = 0;
+      }
+    } else {
+      safetyCounter = 0;
+    }
+  }
+
+  HAL_RemoveNewDataEventHandle(event.GetHandle());
+}
+}  // namespace
+
+static std::atomic_bool gShutdown{false};
+
+namespace {
+struct MotorSafetyManager {
+  ~MotorSafetyManager() { gShutdown = true; }
+
+  wpi::SafeThreadOwner<Thread> thread;
+  wpi::SmallPtrSet<MotorSafety*, 32> instanceList;
+  wpi::mutex listMutex;
+  bool threadStarted = false;
+};
+}  // namespace
+
+static MotorSafetyManager& GetManager() {
+  static MotorSafetyManager manager;
+  return manager;
+}
+
+#ifndef __FRC_SYSTEMCORE__
+namespace frc::impl {
+void ResetMotorSafety() {
+  auto& manager = GetManager();
+  std::scoped_lock lock(manager.listMutex);
+  manager.instanceList.clear();
+  manager.thread.Stop();
+  manager.thread.Join();
+  manager.thread = wpi::SafeThreadOwner<Thread>{};
+  manager.threadStarted = false;
+}
+}  // namespace frc::impl
+#endif
+
+MotorSafety::MotorSafety() {
+  auto& manager = GetManager();
+  std::scoped_lock lock(manager.listMutex);
+  manager.instanceList.insert(this);
+  if (!manager.threadStarted) {
+    manager.threadStarted = true;
+    manager.thread.Start();
+  }
+}
+
+MotorSafety::~MotorSafety() {
+  auto& manager = GetManager();
+  std::scoped_lock lock(manager.listMutex);
+  manager.instanceList.erase(this);
+}
+
+MotorSafety::MotorSafety(MotorSafety&& rhs)
+    : m_expiration(std::move(rhs.m_expiration)),
+      m_enabled(std::move(rhs.m_enabled)),
+      m_stopTime(std::move(rhs.m_stopTime)) {}
+
+MotorSafety& MotorSafety::operator=(MotorSafety&& rhs) {
+  std::scoped_lock lock(m_thisMutex, rhs.m_thisMutex);
+
+  m_expiration = std::move(rhs.m_expiration);
+  m_enabled = std::move(rhs.m_enabled);
+  m_stopTime = std::move(rhs.m_stopTime);
+
+  return *this;
+}
+
+void MotorSafety::Feed() {
+  std::scoped_lock lock(m_thisMutex);
+  m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
+}
+
+void MotorSafety::SetExpiration(units::second_t expirationTime) {
+  std::scoped_lock lock(m_thisMutex);
+  m_expiration = expirationTime;
+}
+
+units::second_t MotorSafety::GetExpiration() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_expiration;
+}
+
+bool MotorSafety::IsAlive() const {
+  std::scoped_lock lock(m_thisMutex);
+  return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
+}
+
+void MotorSafety::SetSafetyEnabled(bool enabled) {
+  std::scoped_lock lock(m_thisMutex);
+  m_enabled = enabled;
+}
+
+bool MotorSafety::IsSafetyEnabled() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_enabled;
+}
+
+void MotorSafety::Check() {
+  bool enabled;
+  units::second_t stopTime;
+
+  {
+    std::scoped_lock lock(m_thisMutex);
+    enabled = m_enabled;
+    stopTime = m_stopTime;
+  }
+
+  if (!enabled || DriverStation::IsDisabled() || DriverStation::IsTest()) {
+    return;
+  }
+
+  if (stopTime < Timer::GetFPGATimestamp()) {
+    FRC_ReportError(err::Timeout,
+                    "{}... Output not updated often enough. See "
+                    "https://docs.wpilib.org/motorsafety for more information.",
+                    GetDescription());
+
+    try {
+      StopMotor();
+    } catch (frc::RuntimeError& e) {
+      e.Report();
+    } catch (std::exception& e) {
+      FRC_ReportError(err::Error, "{} StopMotor threw unexpected exception: {}",
+                      GetDescription(), e.what());
+    }
+  }
+}
+
+void MotorSafety::CheckMotors() {
+  auto& manager = GetManager();
+  std::scoped_lock lock(manager.listMutex);
+  for (auto elem : manager.instanceList) {
+    elem->Check();
+  }
+}

wpilibc/src/main/native/cpp/hardware/pneumatic/Compressor.cpp

@@ -0,0 +1,98 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/Compressor.h"
+
+#include <frc/PneumaticHub.h>
+
+#include <hal/Ports.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+Compressor::Compressor(int busId, int module, PneumaticsModuleType moduleType)
+    : m_module{PneumaticsBase::GetForType(busId, module, moduleType)},
+      m_moduleType{moduleType} {
+  if (!m_module->ReserveCompressor()) {
+    throw FRC_MakeError(err::ResourceAlreadyAllocated, "{}", module);
+  }
+
+  m_module->EnableCompressorDigital();
+
+  m_module->ReportUsage("Compressor", "");
+  wpi::SendableRegistry::Add(this, "Compressor", module);
+}
+
+Compressor::Compressor(int busId, PneumaticsModuleType moduleType)
+    : Compressor{busId, PneumaticsBase::GetDefaultForType(moduleType),
+                 moduleType} {}
+
+Compressor::~Compressor() {
+  if (m_module) {
+    m_module->UnreserveCompressor();
+  }
+}
+
+bool Compressor::IsEnabled() const {
+  return m_module->GetCompressor();
+}
+
+bool Compressor::GetPressureSwitchValue() const {
+  return m_module->GetPressureSwitch();
+}
+
+units::ampere_t Compressor::GetCurrent() const {
+  return m_module->GetCompressorCurrent();
+}
+
+units::volt_t Compressor::GetAnalogVoltage() const {
+  return m_module->GetAnalogVoltage(0);
+}
+
+units::pounds_per_square_inch_t Compressor::GetPressure() const {
+  return m_module->GetPressure(0);
+}
+
+void Compressor::Disable() {
+  m_module->DisableCompressor();
+}
+
+void Compressor::EnableDigital() {
+  m_module->EnableCompressorDigital();
+}
+
+void Compressor::EnableAnalog(units::pounds_per_square_inch_t minPressure,
+                              units::pounds_per_square_inch_t maxPressure) {
+  m_module->EnableCompressorAnalog(minPressure, maxPressure);
+}
+
+void Compressor::EnableHybrid(units::pounds_per_square_inch_t minPressure,
+                              units::pounds_per_square_inch_t maxPressure) {
+  m_module->EnableCompressorHybrid(minPressure, maxPressure);
+}
+
+CompressorConfigType Compressor::GetConfigType() const {
+  return m_module->GetCompressorConfigType();
+}
+
+void Compressor::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Compressor");
+  builder.AddBooleanProperty(
+      "Enabled", [this] { return IsEnabled(); }, nullptr);
+  builder.AddBooleanProperty(
+      "Pressure switch", [this] { return GetPressureSwitchValue(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Current (A)", [this] { return GetCurrent().value(); }, nullptr);
+  // These are not supported by the CTRE PCM
+  if (m_moduleType == PneumaticsModuleType::REVPH) {
+    builder.AddDoubleProperty(
+        "Analog Voltage", [this] { return GetAnalogVoltage().value(); },
+        nullptr);
+    builder.AddDoubleProperty(
+        "Pressure (PSI)", [this] { return GetPressure().value(); }, nullptr);
+  }
+}

wpilibc/src/main/native/cpp/hardware/pneumatic/DoubleSolenoid.cpp

@@ -0,0 +1,151 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/DoubleSolenoid.h"
+
+#include <utility>
+
+#include <hal/Ports.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+
+DoubleSolenoid::DoubleSolenoid(int busId, int module,
+                               PneumaticsModuleType moduleType,
+                               int forwardChannel, int reverseChannel)
+    : m_module{PneumaticsBase::GetForType(busId, module, moduleType)},
+      m_forwardChannel{forwardChannel},
+      m_reverseChannel{reverseChannel} {
+  if (!m_module->CheckSolenoidChannel(m_forwardChannel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}",
+                        m_forwardChannel);
+  }
+  if (!m_module->CheckSolenoidChannel(m_reverseChannel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}",
+                        m_reverseChannel);
+  }
+
+  m_forwardMask = 1 << forwardChannel;
+  m_reverseMask = 1 << reverseChannel;
+  m_mask = m_forwardMask | m_reverseMask;
+
+  int allocMask = m_module->CheckAndReserveSolenoids(m_mask);
+  if (allocMask != 0) {
+    if (allocMask == m_mask) {
+      throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channels {} and {}",
+                          m_forwardChannel, m_reverseChannel);
+    } else if (allocMask == m_forwardMask) {
+      throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}",
+                          m_forwardChannel);
+    } else {
+      throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}",
+                          m_reverseChannel);
+    }
+  }
+
+  m_module->ReportUsage(
+      fmt::format("Solenoid[{},{}]", m_forwardChannel, m_reverseChannel),
+      "DoubleSolenoid");
+
+  wpi::SendableRegistry::Add(this, "DoubleSolenoid",
+                             m_module->GetModuleNumber(), m_forwardChannel);
+}
+
+DoubleSolenoid::DoubleSolenoid(int busId, PneumaticsModuleType moduleType,
+                               int forwardChannel, int reverseChannel)
+    : DoubleSolenoid{busId, PneumaticsBase::GetDefaultForType(moduleType),
+                     moduleType, forwardChannel, reverseChannel} {}
+
+DoubleSolenoid::~DoubleSolenoid() {
+  if (m_module) {
+    m_module->UnreserveSolenoids(m_mask);
+  }
+}
+
+void DoubleSolenoid::Set(Value value) {
+  int setValue = 0;
+
+  switch (value) {
+    case kOff:
+      setValue = 0;
+      break;
+    case kForward:
+      setValue = m_forwardMask;
+      break;
+    case kReverse:
+      setValue = m_reverseMask;
+      break;
+  }
+
+  m_module->SetSolenoids(m_mask, setValue);
+}
+
+DoubleSolenoid::Value DoubleSolenoid::Get() const {
+  auto values = m_module->GetSolenoids();
+
+  if ((values & m_forwardMask) != 0) {
+    return Value::kForward;
+  } else if ((values & m_reverseMask) != 0) {
+    return Value::kReverse;
+  } else {
+    return Value::kOff;
+  }
+}
+
+void DoubleSolenoid::Toggle() {
+  Value value = Get();
+
+  if (value == kForward) {
+    Set(kReverse);
+  } else if (value == kReverse) {
+    Set(kForward);
+  }
+}
+
+int DoubleSolenoid::GetFwdChannel() const {
+  return m_forwardChannel;
+}
+
+int DoubleSolenoid::GetRevChannel() const {
+  return m_reverseChannel;
+}
+
+bool DoubleSolenoid::IsFwdSolenoidDisabled() const {
+  return (m_module->GetSolenoidDisabledList() & m_forwardMask) != 0;
+}
+
+bool DoubleSolenoid::IsRevSolenoidDisabled() const {
+  return (m_module->GetSolenoidDisabledList() & m_reverseMask) != 0;
+}
+
+void DoubleSolenoid::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Double Solenoid");
+  builder.SetActuator(true);
+  builder.AddSmallStringProperty(
+      "Value",
+      [=, this](wpi::SmallVectorImpl<char>& buf) -> std::string_view {
+        switch (Get()) {
+          case kForward:
+            return "Forward";
+          case kReverse:
+            return "Reverse";
+          default:
+            return "Off";
+        }
+      },
+      [=, this](std::string_view value) {
+        Value lvalue = kOff;
+        if (value == "Forward") {
+          lvalue = kForward;
+        } else if (value == "Reverse") {
+          lvalue = kReverse;
+        }
+        Set(lvalue);
+      });
+}

wpilibc/src/main/native/cpp/hardware/pneumatic/PneumaticHub.cpp

@@ -0,0 +1,447 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PneumaticHub.h"
+
+#include <array>
+#include <cstdio>
+#include <memory>
+#include <string>
+
+#include <fmt/format.h>
+#include <hal/Ports.h>
+#include <hal/REVPH.h>
+#include <hal/UsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/StackTrace.h>
+
+#include "frc/Compressor.h"
+#include "frc/DoubleSolenoid.h"
+#include "frc/Errors.h"
+#include "frc/RobotBase.h"
+#include "frc/SensorUtil.h"
+#include "frc/Solenoid.h"
+
+using namespace frc;
+
+/** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
+units::pounds_per_square_inch_t VoltsToPSI(units::volt_t sensorVoltage,
+                                           units::volt_t supplyVoltage) {
+  return units::pounds_per_square_inch_t{
+      250 * (sensorVoltage.value() / supplyVoltage.value()) - 25};
+}
+
+/** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
+units::volt_t PSIToVolts(units::pounds_per_square_inch_t pressure,
+                         units::volt_t supplyVoltage) {
+  return units::volt_t{supplyVoltage.value() *
+                       (0.004 * pressure.value() + 0.1)};
+}
+
+wpi::mutex PneumaticHub::m_handleLock;
+std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<PneumaticHub::DataStore>>[]>
+    PneumaticHub::m_handleMaps = nullptr;
+
+// Always called under lock, so we can avoid the double lock from the magic
+// static
+std::weak_ptr<PneumaticHub::DataStore>& PneumaticHub::GetDataStore(int busId,
+                                                                   int module) {
+  int32_t numBuses = HAL_GetNumCanBuses();
+  FRC_AssertMessage(busId >= 0 && busId < numBuses,
+                    "Bus {} out of range. Must be [0-{}).", busId, numBuses);
+  if (!m_handleMaps) {
+    m_handleMaps = std::make_unique<
+        wpi::DenseMap<int, std::weak_ptr<PneumaticHub::DataStore>>[]>(numBuses);
+  }
+  return m_handleMaps[busId][module];
+}
+
+class PneumaticHub::DataStore {
+ public:
+  explicit DataStore(int busId, int module, const char* stackTrace) {
+    int32_t status = 0;
+    HAL_REVPHHandle handle =
+        HAL_InitializeREVPH(busId, module, stackTrace, &status);
+    FRC_CheckErrorStatus(status, "Module {}", module);
+    m_moduleObject = PneumaticHub{busId, handle, module};
+    m_moduleObject.m_dataStore =
+        std::shared_ptr<DataStore>{this, wpi::NullDeleter<DataStore>()};
+
+    auto version = m_moduleObject.GetVersion();
+
+    // Check PH firmware version
+    if (version.FirmwareMajor > 0 && version.FirmwareMajor < 22) {
+      throw FRC_MakeError(
+          err::AssertionFailure,
+          "The Pneumatic Hub has firmware version {}.{}.{}, and must be "
+          "updated to version 2022.0.0 or later using the REV Hardware Client",
+          version.FirmwareMajor, version.FirmwareMinor, version.FirmwareFix);
+    }
+  }
+
+  ~DataStore() noexcept { HAL_FreeREVPH(m_moduleObject.m_handle); }
+
+  DataStore(DataStore&&) = delete;
+  DataStore& operator=(DataStore&&) = delete;
+
+ private:
+  friend class PneumaticHub;
+  uint32_t m_reservedMask{0};
+  bool m_compressorReserved{false};
+  wpi::mutex m_reservedLock;
+  PneumaticHub m_moduleObject{0, HAL_kInvalidHandle, 0};
+  std::array<units::millisecond_t, 16> m_oneShotDurMs{0_ms};
+};
+
+PneumaticHub::PneumaticHub(int busId)
+    : PneumaticHub{busId, SensorUtil::GetDefaultREVPHModule()} {}
+
+PneumaticHub::PneumaticHub(int busId, int module) {
+  std::string stackTrace = wpi::GetStackTrace(1);
+  std::scoped_lock lock(m_handleLock);
+  auto& res = GetDataStore(busId, module);
+  m_dataStore = res.lock();
+  if (!m_dataStore) {
+    m_dataStore =
+        std::make_shared<DataStore>(busId, module, stackTrace.c_str());
+    res = m_dataStore;
+  }
+  m_handle = m_dataStore->m_moduleObject.m_handle;
+  m_module = module;
+}
+
+PneumaticHub::PneumaticHub(int /* busId */, HAL_REVPHHandle handle, int module)
+    : m_handle{handle}, m_module{module} {}
+
+bool PneumaticHub::GetCompressor() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHCompressor(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+
+void PneumaticHub::DisableCompressor() {
+  int32_t status = 0;
+  HAL_SetREVPHClosedLoopControlDisabled(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticHub::EnableCompressorDigital() {
+  int32_t status = 0;
+  HAL_SetREVPHClosedLoopControlDigital(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticHub::EnableCompressorAnalog(
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
+  if (minPressure >= maxPressure) {
+    throw FRC_MakeError(err::InvalidParameter,
+                        "maxPressure must be greater than minPressure");
+  }
+  if (minPressure < 0_psi || minPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "minPressure must be between 0 and 120 PSI, got {}",
+                        minPressure);
+  }
+  if (maxPressure < 0_psi || maxPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "maxPressure must be between 0 and 120 PSI, got {}",
+                        maxPressure);
+  }
+
+  // Send the voltage as it would be if the 5V rail was at exactly 5V.
+  // The firmware will compensate for the real 5V rail voltage, which
+  // can fluctuate somewhat over time.
+  units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
+  units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
+
+  int32_t status = 0;
+  HAL_SetREVPHClosedLoopControlAnalog(m_handle, minAnalogVoltage.value(),
+                                      maxAnalogVoltage.value(), &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticHub::EnableCompressorHybrid(
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
+  if (minPressure >= maxPressure) {
+    throw FRC_MakeError(err::InvalidParameter,
+                        "maxPressure must be greater than minPressure");
+  }
+  if (minPressure < 0_psi || minPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "minPressure must be between 0 and 120 PSI, got {}",
+                        minPressure);
+  }
+  if (maxPressure < 0_psi || maxPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "maxPressure must be between 0 and 120 PSI, got {}",
+                        maxPressure);
+  }
+
+  // Send the voltage as it would be if the 5V rail was at exactly 5V.
+  // The firmware will compensate for the real 5V rail voltage, which
+  // can fluctuate somewhat over time.
+  units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
+  units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
+
+  int32_t status = 0;
+  HAL_SetREVPHClosedLoopControlHybrid(m_handle, minAnalogVoltage.value(),
+                                      maxAnalogVoltage.value(), &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+CompressorConfigType PneumaticHub::GetCompressorConfigType() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHCompressorConfig(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return static_cast<CompressorConfigType>(result);
+}
+
+bool PneumaticHub::GetPressureSwitch() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHPressureSwitch(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+
+units::ampere_t PneumaticHub::GetCompressorCurrent() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHCompressorCurrent(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::ampere_t{result};
+}
+
+void PneumaticHub::SetSolenoids(int mask, int values) {
+  int32_t status = 0;
+  HAL_SetREVPHSolenoids(m_handle, mask, values, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+int PneumaticHub::GetSolenoids() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHSolenoids(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+
+int PneumaticHub::GetModuleNumber() const {
+  return m_module;
+}
+
+int PneumaticHub::GetSolenoidDisabledList() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHSolenoidDisabledList(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+
+void PneumaticHub::FireOneShot(int index) {
+  int32_t status = 0;
+  HAL_FireREVPHOneShot(m_handle, index,
+                       m_dataStore->m_oneShotDurMs[index].value(), &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticHub::SetOneShotDuration(int index, units::second_t duration) {
+  m_dataStore->m_oneShotDurMs[index] = duration;
+}
+
+bool PneumaticHub::CheckSolenoidChannel(int channel) const {
+  return HAL_CheckREVPHSolenoidChannel(channel);
+}
+
+int PneumaticHub::CheckAndReserveSolenoids(int mask) {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  uint32_t uMask = static_cast<uint32_t>(mask);
+  if ((m_dataStore->m_reservedMask & uMask) != 0) {
+    return m_dataStore->m_reservedMask & uMask;
+  }
+  m_dataStore->m_reservedMask |= uMask;
+  return 0;
+}
+
+void PneumaticHub::UnreserveSolenoids(int mask) {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  m_dataStore->m_reservedMask &= ~(static_cast<uint32_t>(mask));
+}
+
+bool PneumaticHub::ReserveCompressor() {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  if (m_dataStore->m_compressorReserved) {
+    return false;
+  }
+  m_dataStore->m_compressorReserved = true;
+  return true;
+}
+
+void PneumaticHub::UnreserveCompressor() {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  m_dataStore->m_compressorReserved = false;
+}
+
+PneumaticHub::Version PneumaticHub::GetVersion() const {
+  int32_t status = 0;
+  HAL_REVPHVersion halVersions;
+  std::memset(&halVersions, 0, sizeof(halVersions));
+  HAL_GetREVPHVersion(m_handle, &halVersions, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PneumaticHub::Version versions;
+  static_assert(sizeof(halVersions) == sizeof(versions));
+  static_assert(std::is_standard_layout_v<decltype(versions)>);
+  static_assert(std::is_trivial_v<decltype(versions)>);
+  std::memcpy(&versions, &halVersions, sizeof(versions));
+  return versions;
+}
+
+PneumaticHub::Faults PneumaticHub::GetFaults() const {
+  int32_t status = 0;
+  HAL_REVPHFaults halFaults;
+  std::memset(&halFaults, 0, sizeof(halFaults));
+  HAL_GetREVPHFaults(m_handle, &halFaults, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PneumaticHub::Faults faults;
+  static_assert(sizeof(halFaults) == sizeof(faults));
+  static_assert(std::is_standard_layout_v<decltype(faults)>);
+  static_assert(std::is_trivial_v<decltype(faults)>);
+  std::memcpy(&faults, &halFaults, sizeof(faults));
+  return faults;
+}
+
+PneumaticHub::StickyFaults PneumaticHub::GetStickyFaults() const {
+  int32_t status = 0;
+  HAL_REVPHStickyFaults halStickyFaults;
+  std::memset(&halStickyFaults, 0, sizeof(halStickyFaults));
+  HAL_GetREVPHStickyFaults(m_handle, &halStickyFaults, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PneumaticHub::StickyFaults stickyFaults;
+  static_assert(sizeof(halStickyFaults) == sizeof(stickyFaults));
+  static_assert(std::is_standard_layout_v<decltype(stickyFaults)>);
+  static_assert(std::is_trivial_v<decltype(stickyFaults)>);
+  std::memcpy(&stickyFaults, &halStickyFaults, sizeof(stickyFaults));
+  return stickyFaults;
+}
+
+bool PneumaticHub::Faults::GetChannelFault(int channel) const {
+  switch (channel) {
+    case 0:
+      return Channel0Fault != 0;
+    case 1:
+      return Channel1Fault != 0;
+    case 2:
+      return Channel2Fault != 0;
+    case 3:
+      return Channel3Fault != 0;
+    case 4:
+      return Channel4Fault != 0;
+    case 5:
+      return Channel5Fault != 0;
+    case 6:
+      return Channel6Fault != 0;
+    case 7:
+      return Channel7Fault != 0;
+    case 8:
+      return Channel8Fault != 0;
+    case 9:
+      return Channel9Fault != 0;
+    case 10:
+      return Channel10Fault != 0;
+    case 11:
+      return Channel11Fault != 0;
+    case 12:
+      return Channel12Fault != 0;
+    case 13:
+      return Channel13Fault != 0;
+    case 14:
+      return Channel14Fault != 0;
+    case 15:
+      return Channel15Fault != 0;
+    default:
+      throw FRC_MakeError(err::ChannelIndexOutOfRange,
+                          "Pneumatics fault channel out of bounds!");
+  }
+}
+
+void PneumaticHub::ClearStickyFaults() {
+  int32_t status = 0;
+  HAL_ClearREVPHStickyFaults(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+units::volt_t PneumaticHub::GetInputVoltage() const {
+  int32_t status = 0;
+  auto voltage = HAL_GetREVPHVoltage(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::volt_t{voltage};
+}
+
+units::volt_t PneumaticHub::Get5VRegulatedVoltage() const {
+  int32_t status = 0;
+  auto voltage = HAL_GetREVPH5VVoltage(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::volt_t{voltage};
+}
+
+units::ampere_t PneumaticHub::GetSolenoidsTotalCurrent() const {
+  int32_t status = 0;
+  auto current = HAL_GetREVPHSolenoidCurrent(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::ampere_t{current};
+}
+
+units::volt_t PneumaticHub::GetSolenoidsVoltage() const {
+  int32_t status = 0;
+  auto voltage = HAL_GetREVPHSolenoidVoltage(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::volt_t{voltage};
+}
+
+units::volt_t PneumaticHub::GetAnalogVoltage(int channel) const {
+  int32_t status = 0;
+  auto voltage = HAL_GetREVPHAnalogVoltage(m_handle, channel, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::volt_t{voltage};
+}
+
+units::pounds_per_square_inch_t PneumaticHub::GetPressure(int channel) const {
+  int32_t status = 0;
+  auto sensorVoltage = HAL_GetREVPHAnalogVoltage(m_handle, channel, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  auto supplyVoltage = HAL_GetREVPH5VVoltage(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return VoltsToPSI(units::volt_t{sensorVoltage}, units::volt_t{supplyVoltage});
+}
+
+Solenoid PneumaticHub::MakeSolenoid(int channel) {
+  return Solenoid{m_module, PneumaticsModuleType::REVPH, channel};
+}
+
+DoubleSolenoid PneumaticHub::MakeDoubleSolenoid(int forwardChannel,
+                                                int reverseChannel) {
+  return DoubleSolenoid{m_module, PneumaticsModuleType::REVPH, forwardChannel,
+                        reverseChannel};
+}
+
+Compressor PneumaticHub::MakeCompressor() {
+  return Compressor{m_module, PneumaticsModuleType::REVPH};
+}
+
+void PneumaticHub::ReportUsage(std::string_view device, std::string_view data) {
+  HAL_ReportUsage(fmt::format("PH[{}]/{}", m_module, device), data);
+}
+
+std::shared_ptr<PneumaticsBase> PneumaticHub::GetForModule(int busId,
+                                                           int module) {
+  std::string stackTrace = wpi::GetStackTrace(1);
+  std::scoped_lock lock(m_handleLock);
+  auto& res = GetDataStore(busId, module);
+  std::shared_ptr<DataStore> dataStore = res.lock();
+  if (!dataStore) {
+    dataStore = std::make_shared<DataStore>(busId, module, stackTrace.c_str());
+    res = dataStore;
+  }
+
+  return std::shared_ptr<PneumaticsBase>{dataStore, &dataStore->m_moduleObject};
+}

wpilibc/src/main/native/cpp/hardware/pneumatic/PneumaticsBase.cpp

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PneumaticsBase.h"
+
+#include <memory>
+
+#include <hal/REVPH.h>
+
+#include "frc/Errors.h"
+#include "frc/PneumaticHub.h"
+#include "frc/PneumaticsControlModule.h"
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+
+static_assert(
+    static_cast<int>(CompressorConfigType::Disabled) ==
+    HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kDisabled);
+static_assert(
+    static_cast<int>(CompressorConfigType::Digital) ==
+    HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kDigital);
+static_assert(
+    static_cast<int>(CompressorConfigType::Analog) ==
+    HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kAnalog);
+static_assert(
+    static_cast<int>(CompressorConfigType::Hybrid) ==
+    HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kHybrid);
+
+std::shared_ptr<PneumaticsBase> PneumaticsBase::GetForType(
+    int busId, int module, PneumaticsModuleType moduleType) {
+  if (moduleType == PneumaticsModuleType::CTREPCM) {
+    return PneumaticsControlModule::GetForModule(busId, module);
+  } else if (moduleType == PneumaticsModuleType::REVPH) {
+    return PneumaticHub::GetForModule(busId, module);
+  }
+  throw FRC_MakeError(err::InvalidParameter, "{}",
+                      static_cast<int>(moduleType));
+}
+
+int PneumaticsBase::GetDefaultForType(PneumaticsModuleType moduleType) {
+  if (moduleType == PneumaticsModuleType::CTREPCM) {
+    return SensorUtil::GetDefaultCTREPCMModule();
+  } else if (moduleType == PneumaticsModuleType::REVPH) {
+    return SensorUtil::GetDefaultREVPHModule();
+  }
+  throw FRC_MakeError(err::InvalidParameter, "{}",
+                      static_cast<int>(moduleType));
+}

wpilibc/src/main/native/cpp/hardware/pneumatic/PneumaticsControlModule.cpp

@@ -0,0 +1,318 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PneumaticsControlModule.h"
+
+#include <memory>
+#include <string>
+
+#include <fmt/format.h>
+#include <hal/CTREPCM.h>
+#include <hal/Ports.h>
+#include <hal/UsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/StackTrace.h>
+
+#include "frc/Compressor.h"
+#include "frc/DoubleSolenoid.h"
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+#include "frc/Solenoid.h"
+
+using namespace frc;
+
+wpi::mutex PneumaticsControlModule::m_handleLock;
+std::unique_ptr<
+    wpi::DenseMap<int, std::weak_ptr<PneumaticsControlModule::DataStore>>[]>
+    PneumaticsControlModule::m_handleMaps = nullptr;
+
+// Always called under lock, so we can avoid the double lock from the magic
+// static
+std::weak_ptr<PneumaticsControlModule::DataStore>&
+PneumaticsControlModule::GetDataStore(int busId, int module) {
+  int32_t numBuses = HAL_GetNumCanBuses();
+  FRC_AssertMessage(busId >= 0 && busId < numBuses,
+                    "Bus {} out of range. Must be [0-{}).", busId, numBuses);
+  if (!m_handleMaps) {
+    m_handleMaps = std::make_unique<wpi::DenseMap<
+        int, std::weak_ptr<PneumaticsControlModule::DataStore>>[]>(numBuses);
+  }
+
+  return m_handleMaps[busId][module];
+}
+
+class PneumaticsControlModule::DataStore {
+ public:
+  explicit DataStore(int busId, int module, const char* stackTrace) {
+    int32_t status = 0;
+    HAL_CTREPCMHandle handle =
+        HAL_InitializeCTREPCM(busId, module, stackTrace, &status);
+    FRC_CheckErrorStatus(status, "Module {}", module);
+    m_moduleObject = PneumaticsControlModule{busId, handle, module};
+    m_moduleObject.m_dataStore =
+        std::shared_ptr<DataStore>{this, wpi::NullDeleter<DataStore>()};
+  }
+
+  ~DataStore() noexcept { HAL_FreeCTREPCM(m_moduleObject.m_handle); }
+
+  DataStore(DataStore&&) = delete;
+  DataStore& operator=(DataStore&&) = delete;
+
+ private:
+  friend class PneumaticsControlModule;
+  uint32_t m_reservedMask{0};
+  bool m_compressorReserved{false};
+  wpi::mutex m_reservedLock;
+  PneumaticsControlModule m_moduleObject{0, HAL_kInvalidHandle, 0};
+};
+
+PneumaticsControlModule::PneumaticsControlModule(int busId)
+    : PneumaticsControlModule{busId, SensorUtil::GetDefaultCTREPCMModule()} {}
+
+PneumaticsControlModule::PneumaticsControlModule(int busId, int module) {
+  std::string stackTrace = wpi::GetStackTrace(1);
+  std::scoped_lock lock(m_handleLock);
+  auto& res = GetDataStore(busId, module);
+  m_dataStore = res.lock();
+  if (!m_dataStore) {
+    m_dataStore =
+        std::make_shared<DataStore>(busId, module, stackTrace.c_str());
+    res = m_dataStore;
+  }
+  m_handle = m_dataStore->m_moduleObject.m_handle;
+  m_module = module;
+}
+
+PneumaticsControlModule::PneumaticsControlModule(int /* busId */,
+                                                 HAL_CTREPCMHandle handle,
+                                                 int module)
+    : m_handle{handle}, m_module{module} {}
+
+bool PneumaticsControlModule::GetCompressor() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressor(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+
+void PneumaticsControlModule::DisableCompressor() {
+  int32_t status = 0;
+  HAL_SetCTREPCMClosedLoopControl(m_handle, false, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::EnableCompressorDigital() {
+  int32_t status = 0;
+  HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::EnableCompressorAnalog(
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
+  int32_t status = 0;
+  HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::EnableCompressorHybrid(
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
+  int32_t status = 0;
+  HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+CompressorConfigType PneumaticsControlModule::GetCompressorConfigType() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMClosedLoopControl(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result ? CompressorConfigType::Digital
+                : CompressorConfigType::Disabled;
+}
+
+bool PneumaticsControlModule::GetPressureSwitch() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMPressureSwitch(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+
+units::ampere_t PneumaticsControlModule::GetCompressorCurrent() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorCurrent(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return units::ampere_t{result};
+}
+
+bool PneumaticsControlModule::GetCompressorCurrentTooHighFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorCurrentTooHighFault(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetCompressorCurrentTooHighStickyFault() const {
+  int32_t status = 0;
+  auto result =
+      HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetCompressorShortedFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorShortedFault(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetCompressorShortedStickyFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorShortedStickyFault(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetCompressorNotConnectedFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorNotConnectedFault(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetCompressorNotConnectedStickyFault() const {
+  int32_t status = 0;
+  auto result =
+      HAL_GetCTREPCMCompressorNotConnectedStickyFault(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+
+bool PneumaticsControlModule::GetSolenoidVoltageFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoidVoltageFault(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetSolenoidVoltageStickyFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoidVoltageStickyFault(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+
+void PneumaticsControlModule::ClearAllStickyFaults() {
+  int32_t status = 0;
+  HAL_ClearAllCTREPCMStickyFaults(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::SetSolenoids(int mask, int values) {
+  int32_t status = 0;
+  HAL_SetCTREPCMSolenoids(m_handle, mask, values, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+int PneumaticsControlModule::GetSolenoids() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoids(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+
+int PneumaticsControlModule::GetModuleNumber() const {
+  return m_module;
+}
+
+int PneumaticsControlModule::GetSolenoidDisabledList() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoidDisabledList(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return result;
+}
+
+void PneumaticsControlModule::FireOneShot(int index) {
+  int32_t status = 0;
+  HAL_FireCTREPCMOneShot(m_handle, index, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::SetOneShotDuration(int index,
+                                                 units::second_t duration) {
+  int32_t status = 0;
+  units::millisecond_t millis = duration;
+  HAL_SetCTREPCMOneShotDuration(m_handle, index, millis.to<int32_t>(), &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+bool PneumaticsControlModule::CheckSolenoidChannel(int channel) const {
+  return HAL_CheckCTREPCMSolenoidChannel(channel);
+}
+
+int PneumaticsControlModule::CheckAndReserveSolenoids(int mask) {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  uint32_t uMask = static_cast<uint32_t>(mask);
+  if ((m_dataStore->m_reservedMask & uMask) != 0) {
+    return m_dataStore->m_reservedMask & uMask;
+  }
+  m_dataStore->m_reservedMask |= uMask;
+  return 0;
+}
+
+void PneumaticsControlModule::UnreserveSolenoids(int mask) {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  m_dataStore->m_reservedMask &= ~(static_cast<uint32_t>(mask));
+}
+
+bool PneumaticsControlModule::ReserveCompressor() {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  if (m_dataStore->m_compressorReserved) {
+    return false;
+  }
+  m_dataStore->m_compressorReserved = true;
+  return true;
+}
+
+void PneumaticsControlModule::UnreserveCompressor() {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  m_dataStore->m_compressorReserved = false;
+}
+
+units::volt_t PneumaticsControlModule::GetAnalogVoltage(int channel) const {
+  return 0_V;
+}
+
+units::pounds_per_square_inch_t PneumaticsControlModule::GetPressure(
+    int channel) const {
+  return 0_psi;
+}
+
+Solenoid PneumaticsControlModule::MakeSolenoid(int channel) {
+  return Solenoid{m_module, PneumaticsModuleType::CTREPCM, channel};
+}
+
+DoubleSolenoid PneumaticsControlModule::MakeDoubleSolenoid(int forwardChannel,
+                                                           int reverseChannel) {
+  return DoubleSolenoid{m_module, PneumaticsModuleType::CTREPCM, forwardChannel,
+                        reverseChannel};
+}
+
+Compressor PneumaticsControlModule::MakeCompressor() {
+  return Compressor{m_module, PneumaticsModuleType::CTREPCM};
+}
+
+void PneumaticsControlModule::ReportUsage(std::string_view device,
+                                          std::string_view data) {
+  HAL_ReportUsage(fmt::format("PCM[{}]/{}", m_module, device), data);
+}
+
+std::shared_ptr<PneumaticsBase> PneumaticsControlModule::GetForModule(
+    int busId, int module) {
+  std::string stackTrace = wpi::GetStackTrace(1);
+  std::scoped_lock lock(m_handleLock);
+  auto& res = GetDataStore(busId, module);
+  std::shared_ptr<DataStore> dataStore = res.lock();
+  if (!dataStore) {
+    dataStore = std::make_shared<DataStore>(busId, module, stackTrace.c_str());
+    res = dataStore;
+  }
+
+  return std::shared_ptr<PneumaticsBase>{dataStore, &dataStore->m_moduleObject};
+}

wpilibc/src/main/native/cpp/hardware/pneumatic/Solenoid.cpp

@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/Solenoid.h"
+
+#include <utility>
+
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+
+Solenoid::Solenoid(int busId, int module, PneumaticsModuleType moduleType,
+                   int channel)
+    : m_module{PneumaticsBase::GetForType(busId, module, moduleType)},
+      m_channel{channel} {
+  if (!m_module->CheckSolenoidChannel(m_channel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", m_channel);
+  }
+  m_mask = 1 << channel;
+
+  if (m_module->CheckAndReserveSolenoids(m_mask) != 0) {
+    throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}", m_channel);
+  }
+
+  m_module->ReportUsage(fmt::format("Solenoid[{}]", m_channel), "Solenoid");
+  wpi::SendableRegistry::Add(this, "Solenoid", m_module->GetModuleNumber(),
+                             m_channel);
+}
+
+Solenoid::Solenoid(int busId, PneumaticsModuleType moduleType, int channel)
+    : Solenoid{busId, PneumaticsBase::GetDefaultForType(moduleType), moduleType,
+               channel} {}
+
+Solenoid::~Solenoid() {
+  if (m_module) {
+    m_module->UnreserveSolenoids(m_mask);
+  }
+}
+
+void Solenoid::Set(bool on) {
+  int value = on ? (0xFFFF & m_mask) : 0;
+  m_module->SetSolenoids(m_mask, value);
+}
+
+bool Solenoid::Get() const {
+  int currentAll = m_module->GetSolenoids();
+  return (currentAll & m_mask) != 0;
+}
+
+void Solenoid::Toggle() {
+  Set(!Get());
+}
+
+int Solenoid::GetChannel() const {
+  return m_channel;
+}
+
+bool Solenoid::IsDisabled() const {
+  return (m_module->GetSolenoidDisabledList() & m_mask) != 0;
+}
+
+void Solenoid::SetPulseDuration(units::second_t duration) {
+  m_module->SetOneShotDuration(m_channel, duration);
+}
+
+void Solenoid::StartPulse() {
+  m_module->FireOneShot(m_channel);
+}
+
+void Solenoid::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Solenoid");
+  builder.SetActuator(true);
+  builder.AddBooleanProperty(
+      "Value", [=, this] { return Get(); },
+      [=, this](bool value) { Set(value); });
+}

wpilibc/src/main/native/cpp/hardware/power/PowerDistribution.cpp

@@ -0,0 +1,360 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PowerDistribution.h"
+
+#include <vector>
+
+#include <fmt/format.h>
+#include <hal/Ports.h>
+#include <hal/PowerDistribution.h>
+#include <hal/UsageReporting.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+
+static_assert(static_cast<HAL_PowerDistributionType>(
+                  frc::PowerDistribution::ModuleType::kCTRE) ==
+              HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE);
+static_assert(static_cast<HAL_PowerDistributionType>(
+                  frc::PowerDistribution::ModuleType::kRev) ==
+              HAL_PowerDistributionType::HAL_PowerDistributionType_kRev);
+static_assert(frc::PowerDistribution::kDefaultModule ==
+              HAL_DEFAULT_POWER_DISTRIBUTION_MODULE);
+
+using namespace frc;
+
+PowerDistribution::PowerDistribution(int busId) {
+  auto stack = wpi::GetStackTrace(1);
+
+  int32_t status = 0;
+  m_handle = HAL_InitializePowerDistribution(
+      busId, kDefaultModule,
+      HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic,
+      stack.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Module {}", kDefaultModule);
+  m_module = HAL_GetPowerDistributionModuleNumber(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+
+  if (HAL_GetPowerDistributionType(m_handle, &status) ==
+      HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) {
+    HAL_ReportUsage("PDP", m_module, "");
+  } else {
+    HAL_ReportUsage("PDH", m_module, "");
+  }
+  wpi::SendableRegistry::Add(this, "PowerDistribution", m_module);
+}
+
+PowerDistribution::PowerDistribution(int busId, int module,
+                                     ModuleType moduleType) {
+  auto stack = wpi::GetStackTrace(1);
+
+  int32_t status = 0;
+  m_handle = HAL_InitializePowerDistribution(
+      busId, module, static_cast<HAL_PowerDistributionType>(moduleType),
+      stack.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Module {}", module);
+  m_module = HAL_GetPowerDistributionModuleNumber(m_handle, &status);
+  FRC_ReportError(status, "Module {}", module);
+
+  if (moduleType == ModuleType::kCTRE) {
+    HAL_ReportUsage("PDP_CTRE", m_module, "");
+  } else {
+    HAL_ReportUsage("PDH_REV", m_module, "");
+  }
+  wpi::SendableRegistry::Add(this, "PowerDistribution", m_module);
+}
+
+int PowerDistribution::GetNumChannels() const {
+  int32_t status = 0;
+  int32_t size = HAL_GetPowerDistributionNumChannels(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return size;
+}
+
+double PowerDistribution::GetVoltage() const {
+  int32_t status = 0;
+  double voltage = HAL_GetPowerDistributionVoltage(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return voltage;
+}
+
+double PowerDistribution::GetTemperature() const {
+  int32_t status = 0;
+  double temperature = HAL_GetPowerDistributionTemperature(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return temperature;
+}
+
+double PowerDistribution::GetCurrent(int channel) const {
+  int32_t status = 0;
+  double current =
+      HAL_GetPowerDistributionChannelCurrent(m_handle, channel, &status);
+  FRC_ReportError(status, "Module {} Channel {}", m_module, channel);
+
+  return current;
+}
+
+std::vector<double> PowerDistribution::GetAllCurrents() const {
+  int32_t status = 0;
+  int32_t size = GetNumChannels();
+  std::vector<double> currents(size);
+
+  HAL_GetPowerDistributionAllChannelCurrents(m_handle, currents.data(), size,
+                                             &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return currents;
+}
+
+double PowerDistribution::GetTotalCurrent() const {
+  int32_t status = 0;
+  double current = HAL_GetPowerDistributionTotalCurrent(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return current;
+}
+
+double PowerDistribution::GetTotalPower() const {
+  int32_t status = 0;
+  double power = HAL_GetPowerDistributionTotalPower(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return power;
+}
+
+double PowerDistribution::GetTotalEnergy() const {
+  int32_t status = 0;
+  double energy = HAL_GetPowerDistributionTotalEnergy(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return energy;
+}
+
+void PowerDistribution::ResetTotalEnergy() {
+  int32_t status = 0;
+  HAL_ResetPowerDistributionTotalEnergy(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PowerDistribution::ClearStickyFaults() {
+  int32_t status = 0;
+  HAL_ClearPowerDistributionStickyFaults(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+int PowerDistribution::GetModule() const {
+  return m_module;
+}
+
+PowerDistribution::ModuleType PowerDistribution::GetType() const {
+  int32_t status = 0;
+  auto type = HAL_GetPowerDistributionType(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return static_cast<ModuleType>(type);
+}
+
+bool PowerDistribution::GetSwitchableChannel() const {
+  int32_t status = 0;
+  bool state = HAL_GetPowerDistributionSwitchableChannel(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return state;
+}
+
+void PowerDistribution::SetSwitchableChannel(bool enabled) {
+  int32_t status = 0;
+  HAL_SetPowerDistributionSwitchableChannel(m_handle, enabled, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+PowerDistribution::Version PowerDistribution::GetVersion() const {
+  int32_t status = 0;
+  HAL_PowerDistributionVersion halVersion;
+  std::memset(&halVersion, 0, sizeof(halVersion));
+  HAL_GetPowerDistributionVersion(m_handle, &halVersion, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PowerDistribution::Version version;
+  static_assert(sizeof(halVersion) == sizeof(version));
+  static_assert(std::is_standard_layout_v<decltype(version)>);
+  static_assert(std::is_trivial_v<decltype(version)>);
+  std::memcpy(&version, &halVersion, sizeof(version));
+  return version;
+}
+
+PowerDistribution::Faults PowerDistribution::GetFaults() const {
+  int32_t status = 0;
+  HAL_PowerDistributionFaults halFaults;
+  std::memset(&halFaults, 0, sizeof(halFaults));
+  HAL_GetPowerDistributionFaults(m_handle, &halFaults, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PowerDistribution::Faults faults;
+  static_assert(sizeof(halFaults) == sizeof(faults));
+  static_assert(std::is_standard_layout_v<decltype(faults)>);
+  static_assert(std::is_trivial_v<decltype(faults)>);
+  std::memcpy(&faults, &halFaults, sizeof(faults));
+  return faults;
+}
+
+bool PowerDistribution::Faults::GetBreakerFault(int channel) const {
+  switch (channel) {
+    case 0:
+      return Channel0BreakerFault != 0;
+    case 1:
+      return Channel1BreakerFault != 0;
+    case 2:
+      return Channel2BreakerFault != 0;
+    case 3:
+      return Channel3BreakerFault != 0;
+    case 4:
+      return Channel4BreakerFault != 0;
+    case 5:
+      return Channel5BreakerFault != 0;
+    case 6:
+      return Channel6BreakerFault != 0;
+    case 7:
+      return Channel7BreakerFault != 0;
+    case 8:
+      return Channel8BreakerFault != 0;
+    case 9:
+      return Channel9BreakerFault != 0;
+    case 10:
+      return Channel10BreakerFault != 0;
+    case 11:
+      return Channel11BreakerFault != 0;
+    case 12:
+      return Channel12BreakerFault != 0;
+    case 13:
+      return Channel13BreakerFault != 0;
+    case 14:
+      return Channel14BreakerFault != 0;
+    case 15:
+      return Channel15BreakerFault != 0;
+    case 16:
+      return Channel16BreakerFault != 0;
+    case 17:
+      return Channel17BreakerFault != 0;
+    case 18:
+      return Channel18BreakerFault != 0;
+    case 19:
+      return Channel19BreakerFault != 0;
+    case 20:
+      return Channel20BreakerFault != 0;
+    case 21:
+      return Channel21BreakerFault != 0;
+    case 22:
+      return Channel22BreakerFault != 0;
+    case 23:
+      return Channel23BreakerFault != 0;
+    default:
+      throw FRC_MakeError(err::ChannelIndexOutOfRange,
+                          "Power distribution fault channel out of bounds!");
+  }
+}
+
+bool PowerDistribution::StickyFaults::GetBreakerFault(int channel) const {
+  switch (channel) {
+    case 0:
+      return Channel0BreakerFault != 0;
+    case 1:
+      return Channel1BreakerFault != 0;
+    case 2:
+      return Channel2BreakerFault != 0;
+    case 3:
+      return Channel3BreakerFault != 0;
+    case 4:
+      return Channel4BreakerFault != 0;
+    case 5:
+      return Channel5BreakerFault != 0;
+    case 6:
+      return Channel6BreakerFault != 0;
+    case 7:
+      return Channel7BreakerFault != 0;
+    case 8:
+      return Channel8BreakerFault != 0;
+    case 9:
+      return Channel9BreakerFault != 0;
+    case 10:
+      return Channel10BreakerFault != 0;
+    case 11:
+      return Channel11BreakerFault != 0;
+    case 12:
+      return Channel12BreakerFault != 0;
+    case 13:
+      return Channel13BreakerFault != 0;
+    case 14:
+      return Channel14BreakerFault != 0;
+    case 15:
+      return Channel15BreakerFault != 0;
+    case 16:
+      return Channel16BreakerFault != 0;
+    case 17:
+      return Channel17BreakerFault != 0;
+    case 18:
+      return Channel18BreakerFault != 0;
+    case 19:
+      return Channel19BreakerFault != 0;
+    case 20:
+      return Channel20BreakerFault != 0;
+    case 21:
+      return Channel21BreakerFault != 0;
+    case 22:
+      return Channel22BreakerFault != 0;
+    case 23:
+      return Channel23BreakerFault != 0;
+    default:
+      throw FRC_MakeError(err::ChannelIndexOutOfRange,
+                          "Power distribution fault channel out of bounds!");
+  }
+}
+
+PowerDistribution::StickyFaults PowerDistribution::GetStickyFaults() const {
+  int32_t status = 0;
+  HAL_PowerDistributionStickyFaults halStickyFaults;
+  std::memset(&halStickyFaults, 0, sizeof(halStickyFaults));
+  HAL_GetPowerDistributionStickyFaults(m_handle, &halStickyFaults, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  PowerDistribution::StickyFaults stickyFaults;
+  static_assert(sizeof(halStickyFaults) == sizeof(stickyFaults));
+  static_assert(std::is_standard_layout_v<decltype(stickyFaults)>);
+  static_assert(std::is_trivial_v<decltype(stickyFaults)>);
+  std::memcpy(&stickyFaults, &halStickyFaults, sizeof(stickyFaults));
+  return stickyFaults;
+}
+
+void PowerDistribution::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("PowerDistribution");
+  int numChannels = GetNumChannels();
+  // Use manual reads to avoid printing errors
+  for (int i = 0; i < numChannels; ++i) {
+    builder.AddDoubleProperty(
+        fmt::format("Chan{}", i),
+        [=, this] {
+          int32_t lStatus = 0;
+          return HAL_GetPowerDistributionChannelCurrent(m_handle, i, &lStatus);
+        },
+        nullptr);
+  }
+  builder.AddDoubleProperty(
+      "Voltage",
+      [=, this] {
+        int32_t lStatus = 0;
+        return HAL_GetPowerDistributionVoltage(m_handle, &lStatus);
+      },
+      nullptr);
+  builder.AddDoubleProperty(
+      "TotalCurrent",
+      [=, this] {
+        int32_t lStatus = 0;
+        return HAL_GetPowerDistributionTotalCurrent(m_handle, &lStatus);
+      },
+      nullptr);
+  builder.AddBooleanProperty(
+      "SwitchableChannel",
+      [=, this] {
+        int32_t lStatus = 0;
+        return HAL_GetPowerDistributionSwitchableChannel(m_handle, &lStatus);
+      },
+      [=, this](bool value) {
+        int32_t lStatus = 0;
+        HAL_SetPowerDistributionSwitchableChannel(m_handle, value, &lStatus);
+      });
+}

wpilibc/src/main/native/cpp/hardware/range/SharpIR.cpp

@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/SharpIR.h"
+
+#include <algorithm>
+
+#include <hal/UsageReporting.h>
+#include <units/length.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/AnalogInput.h"
+
+using namespace frc;
+
+SharpIR SharpIR::GP2Y0A02YK0F(int channel) {
+  return SharpIR(channel, 62.28, -1.092, 20_cm, 150_cm);
+}
+
+SharpIR SharpIR::GP2Y0A21YK0F(int channel) {
+  return SharpIR(channel, 26.449, -1.226, 10_cm, 80_cm);
+}
+
+SharpIR SharpIR::GP2Y0A41SK0F(int channel) {
+  return SharpIR(channel, 12.354, -1.07, 4_cm, 30_cm);
+}
+
+SharpIR SharpIR::GP2Y0A51SK0F(int channel) {
+  return SharpIR(channel, 5.2819, -1.161, 2_cm, 15_cm);
+}
+
+SharpIR::SharpIR(int channel, double a, double b, units::meter_t min,
+                 units::meter_t max)
+    : m_sensor(channel), m_A(a), m_B(b), m_min(min), m_max(max) {
+  HAL_ReportUsage("IO", channel, "SharpIR");
+  wpi::SendableRegistry::Add(this, "SharpIR", channel);
+
+  m_simDevice = hal::SimDevice("SharpIR", m_sensor.GetChannel());
+  if (m_simDevice) {
+    m_simRange = m_simDevice.CreateDouble("Range (m)", false, 0.0);
+    m_sensor.SetSimDevice(m_simDevice);
+  }
+}
+
+int SharpIR::GetChannel() const {
+  return m_sensor.GetChannel();
+}
+
+units::meter_t SharpIR::GetRange() const {
+  if (m_simRange) {
+    return std::clamp(units::meter_t{m_simRange.Get()}, m_min, m_max);
+  } else {
+    // Don't allow zero/negative values
+    auto v = std::max(m_sensor.GetVoltage(), 0.00001);
+
+    return std::clamp(units::meter_t{m_A * std::pow(v, m_B) * 1e-2}, m_min,
+                      m_max);
+  }
+}
+
+void SharpIR::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Ultrasonic");
+  builder.AddDoubleProperty(
+      "Value", [=, this] { return GetRange().value(); }, nullptr);
+}

wpilibc/src/main/native/cpp/hardware/rotation/AnalogEncoder.cpp

@@ -0,0 +1,128 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/AnalogEncoder.h"
+
+#include <utility>
+
+#include <hal/UsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/AnalogInput.h"
+#include "frc/MathUtil.h"
+#include "frc/RobotController.h"
+
+using namespace frc;
+
+AnalogEncoder::~AnalogEncoder() {}
+
+AnalogEncoder::AnalogEncoder(int channel)
+    : AnalogEncoder(std::make_shared<AnalogInput>(channel)) {}
+
+AnalogEncoder::AnalogEncoder(AnalogInput& analogInput)
+    : m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}} {
+  Init(1.0, 0.0);
+}
+
+AnalogEncoder::AnalogEncoder(AnalogInput* analogInput)
+    : m_analogInput{analogInput, wpi::NullDeleter<AnalogInput>{}} {
+  Init(1.0, 0.0);
+}
+
+AnalogEncoder::AnalogEncoder(std::shared_ptr<AnalogInput> analogInput)
+    : m_analogInput{std::move(analogInput)} {
+  Init(1.0, 0.0);
+}
+
+AnalogEncoder::AnalogEncoder(int channel, double fullRange, double expectedZero)
+    : AnalogEncoder(std::make_shared<AnalogInput>(channel), fullRange,
+                    expectedZero) {}
+
+AnalogEncoder::AnalogEncoder(AnalogInput& analogInput, double fullRange,
+                             double expectedZero)
+    : m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}} {
+  Init(fullRange, expectedZero);
+}
+
+AnalogEncoder::AnalogEncoder(AnalogInput* analogInput, double fullRange,
+                             double expectedZero)
+    : m_analogInput{analogInput, wpi::NullDeleter<AnalogInput>{}} {
+  Init(fullRange, expectedZero);
+}
+
+AnalogEncoder::AnalogEncoder(std::shared_ptr<AnalogInput> analogInput,
+                             double fullRange, double expectedZero)
+    : m_analogInput{std::move(analogInput)} {
+  Init(fullRange, expectedZero);
+}
+
+void AnalogEncoder::Init(double fullRange, double expectedZero) {
+  m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()};
+
+  if (m_simDevice) {
+    m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
+  }
+
+  m_fullRange = fullRange;
+  m_expectedZero = expectedZero;
+
+  HAL_ReportUsage("IO", m_analogInput->GetChannel(), "AnalogEncoder");
+
+  wpi::SendableRegistry::Add(this, "Analog Encoder",
+                             m_analogInput->GetChannel());
+}
+
+double AnalogEncoder::Get() const {
+  if (m_simPosition) {
+    return m_simPosition.Get();
+  }
+
+  double analog = m_analogInput->GetVoltage();
+  double pos = analog / RobotController::GetVoltage3V3();
+
+  // Map sensor range if range isn't full
+  pos = MapSensorRange(pos);
+
+  // Compute full range and offset
+  pos = pos * m_fullRange - m_expectedZero;
+
+  // Map from 0 - Full Range
+  double result = InputModulus(pos, 0.0, m_fullRange);
+  // Invert if necessary
+  if (m_isInverted) {
+    return m_fullRange - result;
+  }
+  return result;
+}
+
+void AnalogEncoder::SetVoltagePercentageRange(double min, double max) {
+  m_sensorMin = std::clamp(min, 0.0, 1.0);
+  m_sensorMax = std::clamp(max, 0.0, 1.0);
+}
+
+void AnalogEncoder::SetInverted(bool inverted) {
+  m_isInverted = inverted;
+}
+
+int AnalogEncoder::GetChannel() const {
+  return m_analogInput->GetChannel();
+}
+
+double AnalogEncoder::MapSensorRange(double pos) const {
+  if (pos < m_sensorMin) {
+    pos = m_sensorMin;
+  }
+  if (pos > m_sensorMax) {
+    pos = m_sensorMax;
+  }
+  pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
+  return pos;
+}
+
+void AnalogEncoder::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("AbsoluteEncoder");
+  builder.AddDoubleProperty(
+      "Position", [this] { return this->Get(); }, nullptr);
+}

wpilibc/src/main/native/cpp/hardware/rotation/AnalogPotentiometer.cpp

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/AnalogPotentiometer.h"
+
+#include <memory>
+#include <utility>
+
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/RobotController.h"
+
+using namespace frc;
+
+AnalogPotentiometer::AnalogPotentiometer(int channel, double fullRange,
+                                         double offset)
+    : AnalogPotentiometer(std::make_shared<AnalogInput>(channel), fullRange,
+                          offset) {
+  wpi::SendableRegistry::AddChild(this, m_analog_input.get());
+}
+
+AnalogPotentiometer::AnalogPotentiometer(AnalogInput* input, double fullRange,
+                                         double offset)
+    : AnalogPotentiometer(
+          std::shared_ptr<AnalogInput>(input, wpi::NullDeleter<AnalogInput>()),
+          fullRange, offset) {}
+
+AnalogPotentiometer::AnalogPotentiometer(std::shared_ptr<AnalogInput> input,
+                                         double fullRange, double offset)
+    : m_analog_input(std::move(input)),
+      m_fullRange(fullRange),
+      m_offset(offset) {
+  wpi::SendableRegistry::Add(this, "AnalogPotentiometer",
+                             m_analog_input->GetChannel());
+}
+
+double AnalogPotentiometer::Get() const {
+  return (m_analog_input->GetVoltage() / RobotController::GetVoltage3V3()) *
+             m_fullRange +
+         m_offset;
+}
+
+void AnalogPotentiometer::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Analog Input");
+  builder.AddDoubleProperty("Value", [=, this] { return Get(); }, nullptr);
+}

wpilibc/src/main/native/cpp/hardware/rotation/DutyCycle.cpp

@@ -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.
+
+#include "frc/DutyCycle.h"
+
+#include <string>
+#include <utility>
+
+#include <hal/DutyCycle.h>
+#include <hal/HALBase.h>
+#include <hal/UsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+
+DutyCycle::DutyCycle(int channel) : m_channel{channel} {
+  if (!SensorUtil::CheckDigitalChannel(channel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel);
+  }
+  InitDutyCycle();
+}
+
+void DutyCycle::InitDutyCycle() {
+  int32_t status = 0;
+  std::string stackTrace = wpi::GetStackTrace(1);
+  m_handle = HAL_InitializeDutyCycle(m_channel, stackTrace.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
+  HAL_ReportUsage("IO", m_channel, "DutyCycle");
+  wpi::SendableRegistry::Add(this, "Duty Cycle", m_channel);
+}
+
+units::hertz_t DutyCycle::GetFrequency() const {
+  int32_t status = 0;
+  auto retVal = HAL_GetDutyCycleFrequency(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
+  return units::hertz_t{retVal};
+}
+
+double DutyCycle::GetOutput() const {
+  int32_t status = 0;
+  auto retVal = HAL_GetDutyCycleOutput(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
+  return retVal;
+}
+
+units::second_t DutyCycle::GetHighTime() const {
+  int32_t status = 0;
+  auto retVal = HAL_GetDutyCycleHighTime(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
+  return units::nanosecond_t{static_cast<double>(retVal)};
+}
+
+int DutyCycle::GetSourceChannel() const {
+  return m_channel;
+}
+
+void DutyCycle::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Duty Cycle");
+  builder.AddDoubleProperty(
+      "Frequency", [this] { return this->GetFrequency().value(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Output", [this] { return this->GetOutput(); }, nullptr);
+}

wpilibc/src/main/native/cpp/hardware/rotation/DutyCycleEncoder.cpp

@@ -0,0 +1,166 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/DutyCycleEncoder.h"
+
+#include <memory>
+#include <utility>
+
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/DutyCycle.h"
+#include "frc/MathUtil.h"
+
+using namespace frc;
+
+DutyCycleEncoder::DutyCycleEncoder(int channel)
+    : m_dutyCycle{std::make_shared<DutyCycle>(channel)} {
+  Init(1.0, 0.0);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
+    : m_dutyCycle{&dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
+  Init(1.0, 0.0);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle)
+    : m_dutyCycle{dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
+  Init(1.0, 0.0);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle)
+    : m_dutyCycle{std::move(dutyCycle)} {
+  Init(1.0, 0.0);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(int channel, double fullRange,
+                                   double expectedZero)
+    : m_dutyCycle{std::make_shared<DutyCycle>(channel)} {
+  Init(fullRange, expectedZero);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle, double fullRange,
+                                   double expectedZero)
+    : m_dutyCycle{&dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
+  Init(fullRange, expectedZero);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle, double fullRange,
+                                   double expectedZero)
+    : m_dutyCycle{dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
+  Init(fullRange, expectedZero);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle,
+                                   double fullRange, double expectedZero)
+    : m_dutyCycle{std::move(dutyCycle)} {
+  Init(fullRange, expectedZero);
+}
+
+void DutyCycleEncoder::Init(double fullRange, double expectedZero) {
+  m_simDevice = hal::SimDevice{"DutyCycle:DutyCycleEncoder",
+                               m_dutyCycle->GetSourceChannel()};
+
+  if (m_simDevice) {
+    m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
+    m_simIsConnected =
+        m_simDevice.CreateBoolean("Connected", hal::SimDevice::kInput, true);
+  }
+
+  m_fullRange = fullRange;
+  m_expectedZero = expectedZero;
+
+  wpi::SendableRegistry::Add(this, "DutyCycle Encoder",
+                             m_dutyCycle->GetSourceChannel());
+}
+
+double DutyCycleEncoder::Get() const {
+  if (m_simPosition) {
+    return m_simPosition.Get();
+  }
+
+  double pos;
+  // Compute output percentage (0-1)
+  if (m_period.value() == 0.0) {
+    pos = m_dutyCycle->GetOutput();
+  } else {
+    auto highTime = m_dutyCycle->GetHighTime();
+    pos = highTime / m_period;
+  }
+
+  // Map sensor range if range isn't full
+  pos = MapSensorRange(pos);
+
+  // Compute full range and offset
+  pos = pos * m_fullRange - m_expectedZero;
+
+  // Map from 0 - Full Range
+  double result = InputModulus(pos, 0.0, m_fullRange);
+  // Invert if necessary
+  if (m_isInverted) {
+    return m_fullRange - result;
+  }
+  return result;
+}
+
+double DutyCycleEncoder::MapSensorRange(double pos) const {
+  if (pos < m_sensorMin) {
+    pos = m_sensorMin;
+  }
+  if (pos > m_sensorMax) {
+    pos = m_sensorMax;
+  }
+  pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
+  return pos;
+}
+
+void DutyCycleEncoder::SetDutyCycleRange(double min, double max) {
+  m_sensorMin = std::clamp(min, 0.0, 1.0);
+  m_sensorMax = std::clamp(max, 0.0, 1.0);
+}
+
+units::hertz_t DutyCycleEncoder::GetFrequency() const {
+  return m_dutyCycle->GetFrequency();
+}
+
+bool DutyCycleEncoder::IsConnected() const {
+  if (m_simIsConnected) {
+    return m_simIsConnected.Get();
+  }
+  return GetFrequency() > m_frequencyThreshold;
+}
+
+void DutyCycleEncoder::SetConnectedFrequencyThreshold(
+    units::hertz_t frequency) {
+  if (frequency < 0_Hz) {
+    frequency = 0_Hz;
+  }
+  m_frequencyThreshold = frequency;
+}
+
+void DutyCycleEncoder::SetInverted(bool inverted) {
+  m_isInverted = inverted;
+}
+
+void DutyCycleEncoder::SetAssumedFrequency(units::hertz_t frequency) {
+  if (frequency.value() == 0) {
+    m_period = 0_s;
+  } else {
+    m_period = 1.0 / frequency;
+  }
+}
+
+int DutyCycleEncoder::GetSourceChannel() const {
+  return m_dutyCycle->GetSourceChannel();
+}
+
+void DutyCycleEncoder::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("AbsoluteEncoder");
+  builder.AddDoubleProperty(
+      "Position", [this] { return this->Get(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Is Connected", [this] { return this->IsConnected(); }, nullptr);
+}

wpilibc/src/main/native/cpp/hardware/rotation/Encoder.cpp

@@ -0,0 +1,196 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/Encoder.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/Encoder.h>
+#include <hal/UsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+Encoder::Encoder(int aChannel, int bChannel, bool reverseDirection,
+                 EncodingType encodingType) {
+  InitEncoder(aChannel, bChannel, reverseDirection, encodingType);
+}
+
+int Encoder::Get() const {
+  int32_t status = 0;
+  int value = HAL_GetEncoder(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "Get");
+  return value;
+}
+
+void Encoder::Reset() {
+  int32_t status = 0;
+  HAL_ResetEncoder(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "Reset");
+}
+
+units::second_t Encoder::GetPeriod() const {
+  int32_t status = 0;
+  double value = HAL_GetEncoderPeriod(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "GetPeriod");
+  return units::second_t{value};
+}
+
+void Encoder::SetMaxPeriod(units::second_t maxPeriod) {
+  int32_t status = 0;
+  HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod.value(), &status);
+  FRC_CheckErrorStatus(status, "SetMaxPeriod");
+}
+
+bool Encoder::GetStopped() const {
+  int32_t status = 0;
+  bool value = HAL_GetEncoderStopped(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "GetStopped");
+  return value;
+}
+
+bool Encoder::GetDirection() const {
+  int32_t status = 0;
+  bool value = HAL_GetEncoderDirection(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "GetDirection");
+  return value;
+}
+
+int Encoder::GetRaw() const {
+  int32_t status = 0;
+  int value = HAL_GetEncoderRaw(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "GetRaw");
+  return value;
+}
+
+int Encoder::GetEncodingScale() const {
+  int32_t status = 0;
+  int val = HAL_GetEncoderEncodingScale(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "GetEncodingScale");
+  return val;
+}
+
+double Encoder::GetDistance() const {
+  int32_t status = 0;
+  double value = HAL_GetEncoderDistance(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "GetDistance");
+  return value;
+}
+
+double Encoder::GetRate() const {
+  int32_t status = 0;
+  double value = HAL_GetEncoderRate(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "GetRate");
+  return value;
+}
+
+void Encoder::SetMinRate(double minRate) {
+  int32_t status = 0;
+  HAL_SetEncoderMinRate(m_encoder, minRate, &status);
+  FRC_CheckErrorStatus(status, "SetMinRate");
+}
+
+void Encoder::SetDistancePerPulse(double distancePerPulse) {
+  int32_t status = 0;
+  HAL_SetEncoderDistancePerPulse(m_encoder, distancePerPulse, &status);
+  FRC_CheckErrorStatus(status, "SetDistancePerPulse");
+}
+
+double Encoder::GetDistancePerPulse() const {
+  int32_t status = 0;
+  double distancePerPulse = HAL_GetEncoderDistancePerPulse(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "GetDistancePerPulse");
+  return distancePerPulse;
+}
+
+void Encoder::SetReverseDirection(bool reverseDirection) {
+  int32_t status = 0;
+  HAL_SetEncoderReverseDirection(m_encoder, reverseDirection, &status);
+  FRC_CheckErrorStatus(status, "SetReverseDirection");
+}
+
+void Encoder::SetSamplesToAverage(int samplesToAverage) {
+  if (samplesToAverage < 1 || samplesToAverage > 127) {
+    throw FRC_MakeError(
+        err::ParameterOutOfRange,
+        "Average counter values must be between 1 and 127, got {}",
+        samplesToAverage);
+  }
+  int32_t status = 0;
+  HAL_SetEncoderSamplesToAverage(m_encoder, samplesToAverage, &status);
+  FRC_CheckErrorStatus(status, "SetSamplesToAverage");
+}
+
+int Encoder::GetSamplesToAverage() const {
+  int32_t status = 0;
+  int result = HAL_GetEncoderSamplesToAverage(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "GetSamplesToAverage");
+  return result;
+}
+
+void Encoder::SetSimDevice(HAL_SimDeviceHandle device) {
+  HAL_SetEncoderSimDevice(m_encoder, device);
+}
+
+int Encoder::GetFPGAIndex() const {
+  int32_t status = 0;
+  int val = HAL_GetEncoderFPGAIndex(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "GetFPGAIndex");
+  return val;
+}
+
+void Encoder::InitSendable(wpi::SendableBuilder& builder) {
+  int32_t status = 0;
+  HAL_EncoderEncodingType type = HAL_GetEncoderEncodingType(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "GetEncodingType");
+  if (type == HAL_EncoderEncodingType::HAL_Encoder_k4X) {
+    builder.SetSmartDashboardType("Quadrature Encoder");
+  } else {
+    builder.SetSmartDashboardType("Encoder");
+  }
+
+  builder.AddDoubleProperty("Speed", [=, this] { return GetRate(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Distance", [=, this] { return GetDistance(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Distance per Tick", [=, this] { return GetDistancePerPulse(); },
+      nullptr);
+}
+
+void Encoder::InitEncoder(int aChannel, int bChannel, bool reverseDirection,
+                          EncodingType encodingType) {
+  int32_t status = 0;
+  m_encoder = HAL_InitializeEncoder(
+      aChannel, bChannel, reverseDirection,
+      static_cast<HAL_EncoderEncodingType>(encodingType), &status);
+  FRC_CheckErrorStatus(status, "InitEncoder");
+
+  const char* type = "Encoder";
+  switch (encodingType) {
+    case k1X:
+      type = "Encoder:1x";
+      break;
+    case k2X:
+      type = "Encoder:2x";
+      break;
+    case k4X:
+      type = "Encoder:4x";
+      break;
+  }
+  HAL_ReportUsage(fmt::format("IO[{},{}]", aChannel, bChannel), type);
+  // wpi::SendableRegistry::Add(this, "Encoder", m_aSource->GetChannel());
+}
+
+double Encoder::DecodingScaleFactor() const {
+  int32_t status = 0;
+  double val = HAL_GetEncoderDecodingScaleFactor(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "DecodingScaleFactor");
+  return val;
+}