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