[wpilib] Separate ExpansionHubServo into separate Servo and CRServo classes (#8770)

This removes the confusion of the `ExpansionHubServo` class serving both
purposes, and thus having a `set` method that functions as `setPosition`
when in servo mode and `setThrottle` when not in continuous mode. It
also removes the `setContinuousRotationMethod` which could be confused
for a method that switches the actual servo firmware itself from servo
to continuous mode, which is not a thing that is physically possible I
think.

---------

Signed-off-by: Zach Harel <zach@zharel.me>

wpilibc/robotpy_pybind_build_info.bzl

@@ -449,6 +449,16 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ
                 ("wpi::ExpansionHub", "wpi__ExpansionHub.hpp"),
             ],
         ),
+        struct(
+            class_name = "ExpansionHubCRServo",
+            yml_file = "semiwrap/ExpansionHubCRServo.yml",
+            header_root = "$(execpath :robotpy-native-wpilib.copy_headers)",
+            header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/hardware/expansionhub/ExpansionHubCRServo.hpp",
+            tmpl_class_names = [],
+            trampolines = [
+                ("wpi::ExpansionHubCRServo", "wpi__ExpansionHubCRServo.hpp"),
+            ],
+        ),
         struct(
             class_name = "ExpansionHubMotor",
             yml_file = "semiwrap/ExpansionHubMotor.yml",

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

@@ -30,7 +30,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,105 @@
+// 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 {
+
+/** This class controls a specific servo in continuous rotation mode hooked up
+ * to an ExpansionHub. */
+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

@@ -14,12 +14,14 @@
 
 namespace wpi {
 
-/** This class controls a specific servo hooked up to an ExpansionHub. */
+/** This class controls a specific servo in positional/servo mode hooked up to
+ * an ExpansionHub. */
 class ExpansionHubServo {
  public:
   /**
    * 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 +36,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 +78,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 +104,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 +124,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;

wpilibc/src/main/python/pyproject.toml

@@ -152,6 +152,7 @@ PWM = "wpi/hardware/discrete/PWM.hpp"
 
 # wpi/hardware/expansionhub
 ExpansionHub = "wpi/hardware/expansionhub/ExpansionHub.hpp"
+ExpansionHubCRServo = "wpi/hardware/expansionhub/ExpansionHubCRServo.hpp"
 ExpansionHubMotor = "wpi/hardware/expansionhub/ExpansionHubMotor.hpp"
 ExpansionHubPositionConstants = "wpi/hardware/expansionhub/ExpansionHubPositionConstants.hpp"
 ExpansionHubServo = "wpi/hardware/expansionhub/ExpansionHubServo.hpp"

wpilibc/src/main/python/semiwrap/ExpansionHub.yml

@@ -1,6 +1,7 @@
 extra_includes:
 - wpi/hardware/expansionhub/ExpansionHubMotor.hpp
 - wpi/hardware/expansionhub/ExpansionHubServo.hpp
+- wpi/hardware/expansionhub/ExpansionHubCRServo.hpp
 
 classes:
   wpi::ExpansionHub:
@@ -11,6 +12,7 @@ classes:
     methods:
       ExpansionHub:
       MakeServo:
+      MakeCRServo:
       MakeMotor:
       IsHubConnected:
       GetUsbId:

wpilibc/src/main/python/semiwrap/ExpansionHubCRServo.yml

@@ -0,0 +1,11 @@
+classes:
+  wpi::ExpansionHubCRServo:
+    methods:
+      ExpansionHubCRServo:
+      SetThrottle:
+      SetPulseWidth:
+      SetEnabled:
+      SetFramePeriod:
+      IsHubConnected:
+      SetPWMRange:
+      SetReversed:

wpilibc/src/main/python/semiwrap/ExpansionHubServo.yml

@@ -2,7 +2,7 @@ classes:
   wpi::ExpansionHubServo:
     methods:
       ExpansionHubServo:
-      Set:
+      SetPosition:
       SetAngle:
       SetPulseWidth:
       SetEnabled:
@@ -11,4 +11,3 @@ classes:
       SetAngleRange:
       SetPWMRange:
       SetReversed:
-      SetContinuousRotationMode: