[wpilib] Separate ExpansionHubServo into separate Servo and CRServo classes

README-RobotPy.md

@@ -58,6 +58,7 @@ bazel run //wpilibc:robotpy-wpilib-scan-headers
 ```
 # wpi
 ExpansionHub = "wpi/ExpansionHub.hpp"
+ExpansionHubCRServo = "wpi/ExpansionHubCRServo.hpp"
 ExpansionHubMotor = "wpi/ExpansionHubMotor.hpp"
 ExpansionHubPidConstants = "wpi/ExpansionHubPidConstants.hpp"
 ExpansionHubServo = "wpi/ExpansionHubServo.hpp"

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

@@ -9,6 +9,7 @@
 #include <thread>
 
 #include "wpi/hal/UsageReporting.hpp"
+#include "wpi/hardware/expansionhub/ExpansionHubCRServo.hpp"
 #include "wpi/hardware/expansionhub/ExpansionHubMotor.hpp"
 #include "wpi/hardware/expansionhub/ExpansionHubServo.hpp"
 #include "wpi/nt/BooleanTopic.hpp"
@@ -78,6 +79,10 @@ ExpansionHubMotor ExpansionHub::MakeMotor(int channel) {
   return ExpansionHubMotor{m_dataStore->m_usbId, channel};
 }
 
+ExpansionHubCRServo ExpansionHub::MakeCRServo(int channel) {
+  return ExpansionHubCRServo{m_dataStore->m_usbId, channel};
+}
+
 bool ExpansionHub::IsHubConnected() const {
   return m_dataStore->m_hubConnectedSubscriber.Get(false);
 }

wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubCRServo.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 "wpi/hardware/expansionhub/ExpansionHubCRServo.hpp"
+
+#include "wpi/system/Errors.hpp"
+#include "wpi/system/SystemServer.hpp"
+
+using namespace wpi;
+
+ExpansionHubCRServo::ExpansionHubCRServo(int usbId, int channel)
+    : m_hub{usbId}, m_channel{channel} {
+  WPILIB_AssertMessage(channel >= 0 && channel < ExpansionHub::NumServoPorts,
+                       "ExHub Servo Channel {} out of range", channel);
+
+  if (!m_hub.CheckAndReserveServo(channel)) {
+    throw WPILIB_MakeError(err::ResourceAlreadyAllocated, "Channel {}",
+                           channel);
+  }
+
+  m_hub.ReportUsage(fmt::format("ExHubCRServo[{}]", channel), "ExHubCRServo");
+
+  auto systemServer = SystemServer::GetSystemServer();
+
+  wpi::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);
+}
+
+ExpansionHubCRServo::~ExpansionHubCRServo() noexcept {
+  m_hub.UnreserveServo(m_channel);
+}
+
+void ExpansionHubCRServo::SetThrottle(double value) {
+  value = std::clamp(value, -1.0, 1.0);
+  value = (value + 1.0) / 2.0;
+  if (m_reversed) {
+    value = 1.0 - value;
+  }
+  auto rawValue = ((value * GetFullRangeScaleFactor()) + m_minPwm);
+  SetPulseWidth(rawValue);
+}
+
+void ExpansionHubCRServo::SetPulseWidth(wpi::units::microsecond_t pulseWidth) {
+  SetEnabled(true);
+  m_pulseWidthPublisher.Set(pulseWidth.value());
+}
+
+void ExpansionHubCRServo::SetEnabled(bool enabled) {
+  m_enabledPublisher.Set(enabled);
+}
+
+void ExpansionHubCRServo::SetFramePeriod(
+    wpi::units::microsecond_t framePeriod) {
+  m_framePeriodPublisher.Set(framePeriod.value());
+}
+
+wpi::units::microsecond_t ExpansionHubCRServo::GetFullRangeScaleFactor() const {
+  return m_maxPwm - m_minPwm;
+}
+
+void ExpansionHubCRServo::SetPWMRange(wpi::units::microsecond_t minPwm,
+                                      wpi::units::microsecond_t maxPwm) {
+  if (maxPwm <= minPwm) {
+    throw WPILIB_MakeError(err::ParameterOutOfRange,
+                           "Max PWM must be greater than Min PWM");
+  }
+  m_minPwm = minPwm;
+  m_maxPwm = maxPwm;
+}
+
+void ExpansionHubCRServo::SetReversed(bool reversed) {
+  m_reversed = reversed;
+}

wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp

@@ -28,7 +28,7 @@ ExpansionHubMotor::ExpansionHubMotor(int usbId, int channel)
                            channel);
   }
 
-  m_hub.ReportUsage(fmt::format("ExHubServo[{}]", channel), "ExHubServo");
+  m_hub.ReportUsage(fmt::format("ExHubMotor[{}]", channel), "ExHubMotor");
 
   auto systemServer = SystemServer::GetSystemServer();
 

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

@@ -54,12 +54,7 @@ ExpansionHubServo::~ExpansionHubServo() noexcept {
   m_hub.UnreserveServo(m_channel);
 }
 
-void ExpansionHubServo::Set(double value) {
-  if (m_continuousMode) {
-    value = std::clamp(value, -1.0, 1.0);
-    value = (value + 1.0) / 2.0;
-  }
-
+void ExpansionHubServo::SetPosition(double value) {
   value = std::clamp(value, 0.0, 1.0);
   if (m_reversed) {
     value = 1.0 - value;
@@ -72,7 +67,7 @@ void ExpansionHubServo::SetAngle(wpi::units::degree_t angle) {
   angle = std::clamp(angle, m_minServoAngle, m_maxServoAngle);
 
   // NOLINTNEXTLINE(bugprone-integer-division)
-  Set((angle - m_minServoAngle) / GetServoAngleRange());
+  SetPosition((angle - m_minServoAngle) / GetServoAngleRange());
 }
 
 void ExpansionHubServo::SetPulseWidth(wpi::units::microsecond_t pulseWidth) {
@@ -106,7 +101,9 @@ void ExpansionHubServo::SetPWMRange(wpi::units::microsecond_t minPwm,
   m_maxPwm = maxPwm;
 }
 
-void ExpansionHubServo::SetReversed(bool reversed) {}
+void ExpansionHubServo::SetReversed(const bool reversed) {
+  m_reversed = reversed;
+}
 
 void ExpansionHubServo::SetAngleRange(wpi::units::degree_t minAngle,
                                       wpi::units::degree_t maxAngle) {
@@ -117,7 +114,3 @@ void ExpansionHubServo::SetAngleRange(wpi::units::degree_t minAngle,
   m_minServoAngle = minAngle;
   m_maxServoAngle = maxAngle;
 }
-
-void ExpansionHubServo::SetContinuousRotationMode(bool enable) {
-  m_continuousMode = enable;
-}

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

@@ -12,6 +12,7 @@
 namespace wpi {
 class ExpansionHubServo;
 class ExpansionHubMotor;
+class ExpansionHubCRServo;
 
 /** This class controls a REV ExpansionHub plugged in over USB to Systemcore. */
 class ExpansionHub {
@@ -29,6 +30,7 @@ class ExpansionHub {
 
   friend class ExpansionHubServo;
   friend class ExpansionHubMotor;
+  friend class ExpansionHubCRServo;
 
   /**
    * Constructs a servo at the requested channel on this hub.
@@ -40,6 +42,8 @@ class ExpansionHub {
    */
   ExpansionHubServo MakeServo(int channel);
 
+  ExpansionHubCRServo MakeCRServo(int channel);
+
   /**
    * Constructs a motor at the requested channel on this hub.
    *

wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubCRServo.hpp

@@ -0,0 +1,102 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "wpi/hardware/expansionhub/ExpansionHub.hpp"
+#include "wpi/nt/BooleanTopic.hpp"
+#include "wpi/nt/IntegerTopic.hpp"
+#include "wpi/units/time.hpp"
+
+namespace wpi {
+class ExpansionHubCRServo {
+ public:
+  /**
+   * Constructs a continuous rotation servo at the requested channel on a
+   * specific USB port.
+   *
+   * @sa ExpansionHubServo for a servo mode, or non-continuous rotation servo
+   * @param usbId The USB port ID the hub is connected to
+   * @param channel The servo channel
+   */
+  ExpansionHubCRServo(int usbId, int channel);
+  ~ExpansionHubCRServo() noexcept;
+
+  /**
+   * Set the servo throttle.
+   *
+   * Throttle values range from -1.0 to 1.0 corresponding to full reverse to
+   * full forward.
+   *
+   * @param value Throttle from -1.0 to 1.0.
+   */
+  void SetThrottle(double value);
+
+  /**
+   * Sets the raw pulse width output on the servo.
+   *
+   * @param pulseWidth Pulse width
+   */
+  void SetPulseWidth(wpi::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(wpi::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 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(wpi::units::microsecond_t minPwm,
+                   wpi::units::microsecond_t maxPwm);
+
+  /**
+   * Sets whether the servo is reversed.
+   *
+   * This will reverse SetThrottle.
+   *
+   * @param reversed True to reverse, false for normal
+   */
+  void SetReversed(bool reversed);
+
+ private:
+  wpi::units::microsecond_t GetFullRangeScaleFactor() const;
+
+  ExpansionHub m_hub;
+  int m_channel;
+
+  wpi::units::microsecond_t m_minPwm = 600_us;
+  wpi::units::microsecond_t m_maxPwm = 2400_us;
+
+  bool m_reversed = false;
+
+  wpi::nt::IntegerPublisher m_pulseWidthPublisher;
+  wpi::nt::IntegerPublisher m_framePeriodPublisher;
+  wpi::nt::BooleanPublisher m_enabledPublisher;
+};
+}  // namespace wpi

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

@@ -20,6 +20,7 @@ class ExpansionHubServo {
   /**
    * Constructs a servo at the requested channel on a specific USB port.
    *
+   * @sa ExpansionHubCRServo if the servo is in continuous rotation mode
    * @param usbId The USB port ID the hub is connected to
    * @param channel The servo channel
    */
@@ -34,16 +35,16 @@ class ExpansionHubServo {
    *
    * @param value Position from 0.0 to 1.0.
    */
-  void Set(double value);
+  void SetPosition(double value);
 
   /**
-   * Sets the servo angle
+   * Sets the servo angle.
    *
-   * Servo angles range from 0 to 180 degrees. Use Set() with your own scaler
-   * for other angle ranges.
+   * 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 0 and 180
-   * degrees
+   * @param angle Position in angle units. Will be clamped to be within the
+   * current angle range.
    */
   void SetAngle(wpi::units::degree_t angle);
 
@@ -76,7 +77,7 @@ class ExpansionHubServo {
   bool IsHubConnected() const { return m_hub.IsHubConnected(); }
 
   /**
-   * Sets the angle range for the setAngle call.
+   * Sets the angle range for the SetAngle call.
    * By default, this is 0 to 180 degrees.
    *
    * Maximum angle must be greater than minimum angle.
@@ -102,22 +103,12 @@ class ExpansionHubServo {
   /**
    * Sets whether the servo is reversed.
    *
-   * This will reverse both Set() and SetAngle().
+   * This will reverse both SetPosition() 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 SetContinuousRotationMode(bool enable);
-
  private:
   wpi::units::microsecond_t GetFullRangeScaleFactor();
   wpi::units::degree_t GetServoAngleRange();
@@ -132,7 +123,6 @@ class ExpansionHubServo {
   wpi::units::microsecond_t m_maxPwm = 2400_us;
 
   bool m_reversed = false;
-  bool m_continuousMode = false;
 
   wpi::nt::IntegerPublisher m_pulseWidthPublisher;
   wpi::nt::IntegerPublisher m_framePeriodPublisher;