[wpilib] separate expansion hub position and velocity constants (#8791)

this is because position and velocity control follow different rules;
see #8773

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

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

@@ -45,8 +45,8 @@ public class ExpansionHubMotor implements AutoCloseable {
 
   private final DoublePublisher m_distancePerCountPublisher;
 
-  private final ExpansionHubPidConstants m_velocityPidConstants;
-  private final ExpansionHubPidConstants m_positionPidConstants;
+  private final ExpansionHubVelocityConstants m_velocityPidConstants;
+  private final ExpansionHubPositionConstants m_positionPidConstants;
 
   /**
    * Constructs a servo at the requested channel on a specific USB port.
@@ -124,8 +124,8 @@ public class ExpansionHubMotor implements AutoCloseable {
             .getBooleanTopic("/rhsp/" + usbId + "/motor" + channel + "/resetEncoder")
             .publish(options);
 
-    m_velocityPidConstants = new ExpansionHubPidConstants(usbId, channel, true);
-    m_positionPidConstants = new ExpansionHubPidConstants(usbId, channel, false);
+    m_velocityPidConstants = new ExpansionHubVelocityConstants(usbId, channel);
+    m_positionPidConstants = new ExpansionHubPositionConstants(usbId, channel);
   }
 
   /** Closes a motor so another instance can be constructed. */
@@ -275,20 +275,20 @@ public class ExpansionHubMotor implements AutoCloseable {
   }
 
   /**
-   * Gets the PID constants object for velocity PID.
+   * Gets the motor constants object for velocity PID.
    *
-   * @return Velocity PID constants object
+   * @return Velocity motor constants object
    */
-  public ExpansionHubPidConstants getVelocityPidConstants() {
+  public ExpansionHubVelocityConstants getVelocityConstants() {
     return m_velocityPidConstants;
   }
 
   /**
-   * Gets the PID constants object for position PID.
+   * Gets the motor constants object for position PID.
    *
-   * @return Position PID constants object
+   * @return Position motor constants object
    */
-  public ExpansionHubPidConstants getPositionPidConstants() {
+  public ExpansionHubPositionConstants getPositionConstants() {
     return m_positionPidConstants;
   }
 

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

@@ -10,20 +10,19 @@ import org.wpilib.networktables.NetworkTableInstance;
 import org.wpilib.networktables.PubSubOption;
 import org.wpilib.system.SystemServer;
 
-/** This class contains PID constants for an ExpansionHub motor. */
-public class ExpansionHubPidConstants {
+/** This class contains feedback and feedforward constants for an ExpansionHub motor. */
+public class ExpansionHubPositionConstants {
   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) {
+  ExpansionHubPositionConstants(int hubNumber, int motorNumber) {
     NetworkTableInstance systemServer = SystemServer.getSystemServer();
 
     PubSubOption[] options =
@@ -31,48 +30,39 @@ public class ExpansionHubPidConstants {
           PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES, PubSubOption.periodic(0.005)
         };
 
-    String pidType = isVelocityPid ? "velocity" : "position";
-
     m_pPublisher =
         systemServer
             .getDoubleTopic(
-                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kp")
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/kp")
             .publish(options);
 
     m_iPublisher =
         systemServer
             .getDoubleTopic(
-                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ki")
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/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")
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/kd")
             .publish(options);
 
     m_sPublisher =
         systemServer
             .getDoubleTopic(
-                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ks")
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/ks")
             .publish(options);
 
     m_continuousPublisher =
         systemServer
             .getBooleanTopic(
-                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/continuous")
+                "/rhsp/"
+                    + hubNumber
+                    + "/motor"
+                    + motorNumber
+                    + "/constants/position"
+                    + "/continuous")
             .publish(options);
 
     m_continuousMinimumPublisher =
@@ -82,8 +72,7 @@ public class ExpansionHubPidConstants {
                     + hubNumber
                     + "/motor"
                     + motorNumber
-                    + "/pid/"
-                    + pidType
+                    + "/constants/position"
                     + "/continuousMinimum")
             .publish(options);
 
@@ -94,8 +83,7 @@ public class ExpansionHubPidConstants {
                     + hubNumber
                     + "/motor"
                     + motorNumber
-                    + "/pid/"
-                    + pidType
+                    + "/constants/position"
                     + "/continuousMaximum")
             .publish(options);
   }
@@ -108,11 +96,13 @@ public class ExpansionHubPidConstants {
    * @param p The proportional coefficient.
    * @param i The integral coefficient.
    * @param d The derivative coefficient.
+   * @return This object, for method chaining.
    */
-  public void setPID(double p, double i, double d) {
+  public ExpansionHubPositionConstants setPID(double p, double i, double d) {
     m_pPublisher.set(p);
     m_iPublisher.set(i);
     m_dPublisher.set(d);
+    return this;
   }
 
   /**
@@ -120,16 +110,14 @@ public class ExpansionHubPidConstants {
    *
    * <p>The units should be radians for angular systems and meters for linear systems.
    *
-   * <p>The PID control period is 10ms
+   * <p>The motor 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²).
+   * @return This object, for method chaining.
    */
-  public void setFF(double s, double v, double a) {
+  public ExpansionHubPositionConstants setS(double s) {
     m_sPublisher.set(s);
-    m_vPublisher.set(v);
-    m_aPublisher.set(a);
+    return this;
   }
 
   /**
@@ -140,24 +128,31 @@ public class ExpansionHubPidConstants {
    *
    * @param minimumInput The minimum value expected from the input.
    * @param maximumInput The maximum value expected from the input.
+   * @return This object, for method chaining.
    */
-  public void enableContinuousInput(double minimumInput, double maximumInput) {
+  public ExpansionHubPositionConstants enableContinuousInput(
+      double minimumInput, double maximumInput) {
     m_continuousMaximumPublisher.set(maximumInput);
     m_continuousMinimumPublisher.set(minimumInput);
     m_continuousPublisher.set(true);
+    return this;
   }
 
-  /** Disable continuous input mode. */
-  public void disableContinuousInput() {
+  /**
+   * Disable continuous input mode.
+   *
+   * @return This object, for method chaining.
+   */
+  public ExpansionHubPositionConstants disableContinuousInput() {
     m_continuousPublisher.set(false);
+    return this;
   }
 
   void close() {
+    m_pPublisher.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/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java

@@ -0,0 +1,111 @@
+// 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 org.wpilib.networktables.DoublePublisher;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.networktables.PubSubOption;
+import org.wpilib.system.SystemServer;
+
+/** This class contains feedback and feedforward constants for an ExpansionHub motor. */
+public class ExpansionHubVelocityConstants {
+  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;
+
+  ExpansionHubVelocityConstants(int hubNumber, int motorNumber) {
+    NetworkTableInstance systemServer = SystemServer.getSystemServer();
+
+    PubSubOption[] options =
+        new PubSubOption[] {
+          PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES, PubSubOption.periodic(0.005)
+        };
+
+    m_pPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/kp")
+            .publish(options);
+
+    m_iPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/ki")
+            .publish(options);
+
+    m_dPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/kd")
+            .publish(options);
+
+    m_sPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/ks")
+            .publish(options);
+
+    m_vPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/kv")
+            .publish(options);
+
+    m_aPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/ka")
+            .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.
+   * @return This object, for method chaining.
+   */
+  public ExpansionHubVelocityConstants setPID(double p, double i, double d) {
+    m_pPublisher.set(p);
+    m_iPublisher.set(i);
+    m_dPublisher.set(d);
+    return this;
+  }
+
+  /**
+   * 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 motor control period is 10ms
+   *
+   * @param s The static gain in volts.
+   * @param v The velocity gain in volts per unit per second.
+   * @param a The acceleration gain in volts per unit per second squared.
+   * @return This object, for method chaining.
+   */
+  public ExpansionHubVelocityConstants setFF(double s, double v, double a) {
+    m_sPublisher.set(s);
+    m_vPublisher.set(v);
+    m_aPublisher.set(a);
+    return this;
+  }
+
+  void close() {
+    m_pPublisher.close();
+    m_iPublisher.close();
+    m_dPublisher.close();
+    m_sPublisher.close();
+    m_vPublisher.close();
+    m_aPublisher.close();
+  }
+}