[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>

wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java

@@ -167,6 +167,18 @@ public class ExpansionHub implements AutoCloseable {
     return new ExpansionHubServo(m_dataStore.m_usbId, channel);
   }
 
+  /**
+   * Constructs a continuous rotation 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 Continuous rotation servo object
+   */
+  public ExpansionHubCRServo makeCRServo(int channel) {
+    return new ExpansionHubCRServo(m_dataStore.m_usbId, channel);
+  }
+
   /**
    * Constructs a motor at the requested channel on this hub.
    *

wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubCRServo.java

@@ -0,0 +1,181 @@
+// 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 org.wpilib.hardware.expansionhub;
+
+import static org.wpilib.units.Units.Microseconds;
+
+import org.wpilib.hardware.hal.util.AllocationException;
+import org.wpilib.networktables.BooleanPublisher;
+import org.wpilib.networktables.IntegerPublisher;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.networktables.PubSubOption;
+import org.wpilib.system.SystemServer;
+import org.wpilib.units.measure.Time;
+
+/**
+ * This class controls a specific servo in continuous rotation mode hooked up to an ExpansionHub.
+ */
+public class ExpansionHubCRServo implements AutoCloseable {
+  private ExpansionHub m_hub;
+  private final int m_channel;
+
+  private boolean m_reversed;
+
+  private final IntegerPublisher m_pulseWidthPublisher;
+  private final IntegerPublisher m_framePeriodPublisher;
+  private final BooleanPublisher m_enabledPublisher;
+
+  private int m_minPwm = 600;
+  private int m_maxPwm = 2400;
+
+  /**
+   * Constructs a continuous rotation 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 ExpansionHubCRServo(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 CR Servo already allocated");
+    }
+
+    m_hub.reportUsage("ExHubCRServo[" + channel + "]", "ExHubCRServo");
+
+    NetworkTableInstance systemServer = SystemServer.getSystemServer();
+
+    PubSubOption[] options =
+        new PubSubOption[] {
+          PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES, 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);
+  }
+
+  /** 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();
+  }
+
+  /**
+   * Set the servo throttle.
+   *
+   * <p>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.
+   */
+  public void setThrottle(double value) {
+    value = Math.clamp(value, -1.0, 1.0);
+    value = (value + 1.0) / 2.0;
+
+    if (m_reversed) {
+      value = 1.0 - value;
+    }
+
+    int rawValue = (int) ((value * getFullRangeScaleFactor()) + m_minPwm);
+
+    setPulseWidth(Microseconds.of(rawValue));
+  }
+
+  /**
+   * Sets the raw pulse width output on the servo.
+   *
+   * @param pulseWidth Pulse width
+   */
+  public void setPulseWidth(Time pulseWidth) {
+    setEnabled(true);
+    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 setThrottle().
+   *
+   * @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;
+  }
+
+  private double getFullRangeScaleFactor() {
+    return m_maxPwm - m_minPwm;
+  }
+}

wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubServo.java

@@ -16,13 +16,12 @@ import org.wpilib.system.SystemServer;
 import org.wpilib.units.measure.Angle;
 import org.wpilib.units.measure.Time;
 
-/** 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. */
 public class ExpansionHubServo implements AutoCloseable {
   private ExpansionHub m_hub;
   private final int m_channel;
 
   private boolean m_reversed;
-  private boolean m_continuousMode;
 
   private final IntegerPublisher m_pulseWidthPublisher;
   private final IntegerPublisher m_framePeriodPublisher;
@@ -86,17 +85,11 @@ public class ExpansionHubServo implements AutoCloseable {
   /**
    * 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.
+   * <p>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 (-1 to 1 in CR mode).
+   * @param value Position from 0.0 to 1.0.
    */
-  public void set(double value) {
-    if (m_continuousMode) {
-      value = Math.clamp(value, -1.0, 1.0);
-      value = (value + 1.0) / 2.0;
-    }
-
+  public void setPosition(double value) {
     value = Math.clamp(value, 0.0, 1.0);
 
     if (m_reversed) {
@@ -114,7 +107,7 @@ public class ExpansionHubServo implements AutoCloseable {
    *
    * <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.
+   * @param angle Position in angle units. Will be clamped to be within the current angle range.
    */
   public void setAngle(Angle angle) {
     double dAngle = angle.in(Degrees);
@@ -124,7 +117,7 @@ public class ExpansionHubServo implements AutoCloseable {
       dAngle = m_maxServoAngle;
     }
 
-    set((dAngle - m_minServoAngle) / getServoAngleRange());
+    setPosition((dAngle - m_minServoAngle) / getServoAngleRange());
   }
 
   private double getFullRangeScaleFactor() {
@@ -175,7 +168,7 @@ public class ExpansionHubServo implements AutoCloseable {
   /**
    * Sets whether the servo is reversed.
    *
-   * <p>This will reverse both set() and setAngle().
+   * <p>This will reverse both setPosition() and setAngle().
    *
    * @param reversed True to reverse, false for normal
    */
@@ -215,18 +208,6 @@ public class ExpansionHubServo implements AutoCloseable {
     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 setContinuousRotationMode(boolean enable) {
-    m_continuousMode = enable;
-  }
-
   /** Closes a servo so another instance can be constructed. */
   @Override
   public void close() {