[wpilib] Integrate support for ExpansionHub over USB (#8292)

wpilibc/src/main/native/cpp/ExpansionHub.cpp

@@ -0,0 +1,108 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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/ExpansionHub.h"
+
+#include <memory>
+
+#include <hal/UsageReporting.h>
+#include <networktables/BooleanTopic.h>
+
+#include "frc/Errors.h"
+#include "frc/ExpansionHubMotor.h"
+#include "frc/ExpansionHubServo.h"
+#include "frc/SystemServer.h"
+
+using namespace frc;
+
+wpi::mutex ExpansionHub::m_handleLock;
+std::weak_ptr<ExpansionHub::DataStore> ExpansionHub::m_storeMap[4];
+
+class ExpansionHub::DataStore {
+ public:
+  explicit DataStore(int usbId) : m_usbId{usbId} {
+    auto systemServer = SystemServer::GetSystemServer();
+
+    m_hubConnectedSubscriber =
+        systemServer.GetBooleanTopic(fmt::format("/rhsp/{}/connected", usbId))
+            .Subscribe(false);
+  }
+
+  DataStore(DataStore&) = delete;
+  DataStore(DataStore&&) = delete;
+  DataStore& operator=(DataStore&) = delete;
+  DataStore& operator=(DataStore&&) = delete;
+
+  nt::BooleanSubscriber m_hubConnectedSubscriber;
+
+  uint32_t m_reservedMotorMask{0};
+  uint32_t m_reservedServoMask{0};
+  wpi::mutex m_reservedLock;
+
+  int m_usbId;
+};
+
+std::shared_ptr<ExpansionHub::DataStore> ExpansionHub::GetForUsbId(int usbId) {
+  FRC_AssertMessage(usbId >= 0 && usbId < NumUsbPorts, "USB {} out of range",
+                    usbId);
+  std::scoped_lock lock{m_handleLock};
+  std::weak_ptr<DataStore>& weakStore = m_storeMap[usbId];
+  auto strongStore = weakStore.lock();
+  if (!strongStore) {
+    strongStore = std::make_shared<DataStore>(usbId);
+    weakStore = strongStore;
+  }
+  return strongStore;
+}
+
+ExpansionHub::ExpansionHub(int usbId) : m_dataStore{GetForUsbId(usbId)} {}
+
+ExpansionHubServo ExpansionHub::MakeServo(int channel) {
+  return ExpansionHubServo{m_dataStore->m_usbId, channel};
+}
+
+ExpansionHubMotor ExpansionHub::MakeMotor(int channel) {
+  return ExpansionHubMotor{m_dataStore->m_usbId, channel};
+}
+
+bool ExpansionHub::IsHubConnected() const {
+  return m_dataStore->m_hubConnectedSubscriber.Get(false);
+}
+
+bool ExpansionHub::CheckAndReserveServo(int channel) {
+  int mask = 1 << channel;
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  if ((m_dataStore->m_reservedServoMask & mask) != 0) {
+    return false;
+  }
+  m_dataStore->m_reservedServoMask |= mask;
+  return true;
+}
+
+void ExpansionHub::UnreserveServo(int channel) {
+  int mask = 1 << channel;
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  m_dataStore->m_reservedServoMask &= ~mask;
+}
+
+bool ExpansionHub::CheckAndReserveMotor(int channel) {
+  int mask = 1 << channel;
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  if ((m_dataStore->m_reservedMotorMask & mask) != 0) {
+    return false;
+  }
+  m_dataStore->m_reservedMotorMask |= mask;
+  return true;
+}
+
+void ExpansionHub::UnreserveMotor(int channel) {
+  int mask = 1 << channel;
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  m_dataStore->m_reservedMotorMask &= ~mask;
+}
+
+void ExpansionHub::ReportUsage(std::string_view device, std::string_view data) {
+  HAL_ReportUsage(
+      fmt::format("ExpansionHub[{}]/{}", m_dataStore->m_usbId, device), data);
+}

wpilibc/src/main/native/cpp/ExpansionHubMotor.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/ExpansionHubMotor.h"
+
+#include "frc/Errors.h"
+#include "frc/SystemServer.h"
+
+static constexpr int kPercentageMode = 0;
+static constexpr int kVoltageMode = 1;
+static constexpr int kPositionMode = 2;
+static constexpr int kVelocityMode = 3;
+static constexpr int kFollowerMode = 4;
+
+using namespace frc;
+
+ExpansionHubMotor::ExpansionHubMotor(int usbId, int channel)
+    : m_hub{usbId},
+      m_channel{channel},
+      m_velocityPidConstants{usbId, channel, true},
+      m_positionPidConstants{usbId, channel, false} {
+  FRC_AssertMessage(channel >= 0 && channel < ExpansionHub::NumMotorPorts,
+                    "ExHub Motor Channel {} out of range", channel);
+
+  if (!m_hub.CheckAndReserveMotor(channel)) {
+    throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}", channel);
+  }
+
+  m_hub.ReportUsage(fmt::format("ExHubServo[{}]", channel), "ExHubServo");
+
+  auto systemServer = SystemServer::GetSystemServer();
+
+  nt::PubSubOptions options;
+  options.sendAll = true;
+  options.keepDuplicates = true;
+  options.periodic = 0.005;
+
+  m_encoderSubscriber = systemServer
+                            .GetDoubleTopic(fmt::format(
+                                "/rhsp/{}/motor{}/encoder", usbId, channel))
+                            .Subscribe(0, options);
+  m_encoderVelocitySubscriber =
+      systemServer
+          .GetDoubleTopic(
+              fmt::format("/rhsp/{}/motor{}/encoderVelocity", usbId, channel))
+          .Subscribe(0, options);
+  m_currentSubscriber = systemServer
+                            .GetDoubleTopic(fmt::format(
+                                "/rhsp/{}/motor{}/current", usbId, channel))
+                            .Subscribe(0, options);
+
+  m_setpointPublisher = systemServer
+                            .GetDoubleTopic(fmt::format(
+                                "/rhsp/{}/motor{}/setpoint", usbId, channel))
+                            .Publish(options);
+
+  m_distancePerCountPublisher =
+      systemServer
+          .GetDoubleTopic(
+              fmt::format("/rhsp/{}/motor{}/distancePerCount", usbId, channel))
+          .Publish(options);
+
+  m_floatOn0Publisher = systemServer
+                            .GetBooleanTopic(fmt::format(
+                                "/rhsp/{}/motor{}/floatOn0", usbId, channel))
+                            .Publish(options);
+  m_enabledPublisher = systemServer
+                           .GetBooleanTopic(fmt::format(
+                               "/rhsp/{}/motor{}/enabled", usbId, channel))
+                           .Publish(options);
+
+  m_modePublisher =
+      systemServer
+          .GetIntegerTopic(fmt::format("/rhsp/{}/motor{}/mode", usbId, channel))
+          .Publish(options);
+
+  m_reversedPublisher = systemServer
+                            .GetBooleanTopic(fmt::format(
+                                "/rhsp/{}/motor{}/reversed", usbId, channel))
+                            .Publish(options);
+
+  m_resetEncoderPublisher =
+      systemServer
+          .GetBooleanTopic(
+              fmt::format("/rhsp/{}/motor{}/resetEncoder", usbId, channel))
+          .Publish(options);
+}
+
+ExpansionHubMotor::~ExpansionHubMotor() noexcept {
+  m_hub.UnreserveMotor(m_channel);
+}
+
+void ExpansionHubMotor::SetPercentagePower(double power) {
+  m_modePublisher.Set(kPercentageMode);
+  m_setpointPublisher.Set(power);
+}
+
+void ExpansionHubMotor::SetVoltage(units::volt_t voltage) {
+  m_modePublisher.Set(kVoltageMode);
+  m_setpointPublisher.Set(voltage.to<double>());
+}
+
+void ExpansionHubMotor::SetPositionSetpoint(double setpoint) {
+  m_modePublisher.Set(kPositionMode);
+  m_setpointPublisher.Set(setpoint);
+}
+
+void ExpansionHubMotor::SetVelocitySetpoint(double setpoint) {
+  m_modePublisher.Set(kVelocityMode);
+  m_setpointPublisher.Set(setpoint);
+}
+
+void ExpansionHubMotor::SetEnabled(bool enabled) {
+  m_enabledPublisher.Set(enabled);
+}
+
+void ExpansionHubMotor::SetFloatOn0(bool floatOn0) {
+  m_floatOn0Publisher.Set(floatOn0);
+}
+
+units::ampere_t ExpansionHubMotor::GetCurrent() const {
+  return units::ampere_t{m_currentSubscriber.Get(0)};
+}
+
+void ExpansionHubMotor::SetDistancePerCount(double perCount) {
+  m_distancePerCountPublisher.Set(perCount);
+}
+
+double ExpansionHubMotor::GetEncoderVelocity() const {
+  return m_encoderVelocitySubscriber.Get(0);
+}
+
+double ExpansionHubMotor::GetEncoderPosition() const {
+  return m_encoderSubscriber.Get(0);
+}
+
+void ExpansionHubMotor::SetReversed(bool reversed) {
+  m_reversedPublisher.Set(true);
+}
+
+void ExpansionHubMotor::ResetEncoder() {
+  m_resetEncoderPublisher.Set(true);
+}
+
+ExpansionHubPidConstants& ExpansionHubMotor::GetVelocityPidConstants() {
+  return m_velocityPidConstants;
+}
+
+ExpansionHubPidConstants& ExpansionHubMotor::GetPositionPidConstants() {
+  return m_positionPidConstants;
+}
+
+void ExpansionHubMotor::Follow(const ExpansionHubMotor& leader) {
+  if (m_hub.GetUsbId() != leader.m_hub.GetUsbId()) {
+    throw FRC_MakeError(err::InvalidParameter,
+                        "Cannot follow motor on different hub");
+  }
+  m_modePublisher.Set(kFollowerMode);
+  m_setpointPublisher.Set(leader.m_channel);
+}

wpilibc/src/main/native/cpp/ExpansionHubPidConstants.cpp

@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/ExpansionHubPidConstants.h"
+
+#include <string>
+
+#include "frc/Errors.h"
+#include "frc/SystemServer.h"
+
+using namespace frc;
+
+ExpansionHubPidConstants::ExpansionHubPidConstants(int usbId, int channel,
+                                                   bool isVelocityPid) {
+  auto systemServer = SystemServer::GetSystemServer();
+
+  nt::PubSubOptions options;
+  options.sendAll = true;
+  options.keepDuplicates = true;
+  options.periodic = 0.005;
+
+  std::string pidType = isVelocityPid ? "velocity" : "position";
+
+  m_pPublisher = systemServer
+                     .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/kp",
+                                                 usbId, channel, pidType))
+                     .Publish(options);
+
+  m_iPublisher = systemServer
+                     .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/ki",
+                                                 usbId, channel, pidType))
+                     .Publish(options);
+
+  m_dPublisher = systemServer
+                     .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/kd",
+                                                 usbId, channel, pidType))
+                     .Publish(options);
+
+  m_aPublisher = systemServer
+                     .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/ka",
+                                                 usbId, channel, pidType))
+                     .Publish(options);
+
+  m_vPublisher = systemServer
+                     .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/kv",
+                                                 usbId, channel, pidType))
+                     .Publish(options);
+
+  m_sPublisher = systemServer
+                     .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/ks",
+                                                 usbId, channel, pidType))
+                     .Publish(options);
+
+  m_continuousPublisher =
+      systemServer
+          .GetBooleanTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/continous",
+                                       usbId, channel, pidType))
+          .Publish(options);
+
+  m_continuousMinimumPublisher =
+      systemServer
+          .GetDoubleTopic(
+              fmt::format("/rhsp/{}/motor{}/pid/{}/continuousMinimum", usbId,
+                          channel, pidType))
+          .Publish(options);
+
+  m_continuousMaximumPublisher =
+      systemServer
+          .GetDoubleTopic(
+              fmt::format("/rhsp/{}/motor{}/pid/{}/continousMaximum", usbId,
+                          channel, pidType))
+          .Publish(options);
+}
+
+void ExpansionHubPidConstants::SetPID(double p, double i, double d) {
+  m_pPublisher.Set(p);
+  m_iPublisher.Set(i);
+  m_dPublisher.Set(d);
+}
+
+void ExpansionHubPidConstants::SetFF(double s, double v, double a) {
+  m_sPublisher.Set(s);
+  m_vPublisher.Set(v);
+  m_aPublisher.Set(a);
+}
+
+void ExpansionHubPidConstants::EnableContinousInput(double minimumInput,
+                                                    double maximumInput) {
+  m_continuousMaximumPublisher.Set(maximumInput);
+  m_continuousMinimumPublisher.Set(minimumInput);
+  m_continuousPublisher.Set(true);
+}
+
+void ExpansionHubPidConstants::DisableContinousInput() {
+  m_continuousPublisher.Set(false);
+}

wpilibc/src/main/native/cpp/ExpansionHubServo.cpp

@@ -0,0 +1,121 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/ExpansionHubServo.h"
+
+#include "frc/Errors.h"
+#include "frc/SystemServer.h"
+
+using namespace frc;
+
+ExpansionHubServo::ExpansionHubServo(int usbId, int channel)
+    : m_hub{usbId}, m_channel{channel} {
+  FRC_AssertMessage(channel >= 0 && channel < ExpansionHub::NumServoPorts,
+                    "ExHub Servo Channel {} out of range", channel);
+
+  if (!m_hub.CheckAndReserveServo(channel)) {
+    throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}", channel);
+  }
+
+  m_hub.ReportUsage(fmt::format("ExHubServo[{}]", channel), "ExHubServo");
+
+  auto systemServer = SystemServer::GetSystemServer();
+
+  nt::PubSubOptions options;
+  options.sendAll = true;
+  options.keepDuplicates = true;
+  options.periodic = 0.005;
+
+  m_pulseWidthPublisher =
+      systemServer
+          .GetIntegerTopic(
+              fmt::format("/rhsp/{}/servo{}/pulseWidth", usbId, channel))
+          .Publish(options);
+
+  m_pulseWidthPublisher.Set(1500);
+
+  m_framePeriodPublisher =
+      systemServer
+          .GetIntegerTopic(
+              fmt::format("/rhsp/{}/servo{}/framePeriod", usbId, channel))
+          .Publish(options);
+
+  m_framePeriodPublisher.Set(20000);
+
+  m_enabledPublisher = systemServer
+                           .GetBooleanTopic(fmt::format(
+                               "/rhsp/{}/servo{}/enabled", usbId, channel))
+                           .Publish(options);
+}
+
+ExpansionHubServo::~ExpansionHubServo() noexcept {
+  m_hub.UnreserveServo(m_channel);
+}
+
+void ExpansionHubServo::Set(double value) {
+  if (m_continousMode) {
+    value = std::clamp(value, -1.0, 1.0);
+    value = (value + 1.0) / 2.0;
+  }
+
+  value = std::clamp(value, 0.0, 1.0);
+  if (m_reversed) {
+    value = 1.0 - value;
+  }
+  auto rawValue = ((value * GetFullRangeScaleFactor()) + m_minPwm);
+  SetPulseWidth(rawValue);
+}
+
+void ExpansionHubServo::SetAngle(units::degree_t angle) {
+  angle = std::clamp(angle, m_minServoAngle, m_maxServoAngle);
+
+  // NOLINTNEXTLINE(bugprone-integer-division)
+  Set((angle - m_minServoAngle) / GetServoAngleRange());
+}
+
+void ExpansionHubServo::SetPulseWidth(units::microsecond_t pulseWidth) {
+  m_pulseWidthPublisher.Set(pulseWidth.to<double>());
+}
+
+void ExpansionHubServo::SetEnabled(bool enabled) {
+  m_enabledPublisher.Set(enabled);
+}
+
+void ExpansionHubServo::SetFramePeriod(units::microsecond_t framePeriod) {
+  m_framePeriodPublisher.Set(framePeriod.to<double>());
+}
+
+units::microsecond_t ExpansionHubServo::GetFullRangeScaleFactor() {
+  return m_maxPwm - m_minPwm;
+}
+
+units::degree_t ExpansionHubServo::GetServoAngleRange() {
+  return m_maxServoAngle - m_minServoAngle;
+}
+
+void ExpansionHubServo::SetPWMRange(units::microsecond_t minPwm,
+                                    units::microsecond_t maxPwm) {
+  if (maxPwm <= minPwm) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "Max PWM must be greater than Min PWM");
+  }
+  m_minPwm = minPwm;
+  m_maxPwm = maxPwm;
+}
+
+void ExpansionHubServo::SetReversed(bool reversed) {}
+
+void ExpansionHubServo::SetAngleRange(units::degree_t minAngle,
+                                      units::degree_t maxAngle) {
+  if (maxAngle <= minAngle) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "Max angle must be greater than Min angle");
+  }
+  m_minServoAngle = minAngle;
+  m_maxServoAngle = maxAngle;
+}
+
+void ExpansionHubServo::SetContinousRotationMode(bool enable) {
+  m_continousMode = enable;
+}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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