Completes PID controller preference APIs (#72)

This change brings the API surface of `PIDControllerPreference` and `ProfiledPIDControllerPreference` to parity with the non-deprecated API surface of WPILib.

This change also corrects the initialization of the PD controllers to use the current gain preference values.

Author: edreed

nrgcommon/src/main/java/com/nrg948/preferences/PIDControllerPreference.java

@@ -40,13 +40,16 @@ of this software and associated documentation files (the "Software"), to deal
  * implements {@link Sendable} so that its parameters can be exposed to dashboards and other tools.
  */
 public class PIDControllerPreference extends PreferenceValue implements Sendable {
+  private static final double DEFAULT_PERIOD = 0.02;
+
   private final double defaultP;
   private final double defaultI;
   private final double defaultD;
   private final PIDController controller;
 
   /**
-   * Creates a new PIDControllerPreference with the given group, name, and default PID gains.
+   * Creates a new PIDControllerPreference with the given group, name, and default PID gains. Uses a
+   * default period of 0.02 seconds.
    *
    * @param group The preference group.
    * @param name The preference name.
@@ -56,133 +59,149 @@ public class PIDControllerPreference extends PreferenceValue implements Sendable
    */
   public PIDControllerPreference(
       String group, String name, double defaultP, double defaultI, double defaultD) {
+    this(group, name, defaultP, defaultI, defaultD, DEFAULT_PERIOD);
+  }
+
+  /**
+   * Creates a new PIDControllerPreference with the given group, name, default PID gains, and
+   * period.
+   *
+   * @param group The preference group.
+   * @param name The preference name.
+   * @param defaultP The default P gain.
+   * @param defaultI The default I gain.
+   * @param defaultD The default D gain.
+   * @param period The period for this controller.
+   */
+  public PIDControllerPreference(
+      String group, String name, double defaultP, double defaultI, double defaultD, double period) {
     super(group, name);
     this.defaultP = defaultP;
     this.defaultI = defaultI;
     this.defaultD = defaultD;
-    this.controller = new PIDController(defaultP, defaultI, defaultD);
 
+    // Initialize preference entries with defaults if they do not already exist.
     Preferences.initDouble(getKey() + "/kP", defaultP);
     Preferences.initDouble(getKey() + "/kI", defaultI);
     Preferences.initDouble(getKey() + "/kD", defaultD);
+
+    // Read the effective gains from preferences (existing tuned values or defaults).
+    double p = Preferences.getDouble(getKey() + "/kP", defaultP);
+    double i = Preferences.getDouble(getKey() + "/kI", defaultI);
+    double d = Preferences.getDouble(getKey() + "/kD", defaultD);
+
+    // Construct the controller using the stored preference values.
+    this.controller = new PIDController(p, i, d, period);
   }
 
-  /**
-   * Returns the default P value.
-   *
-   * @return The default P value.
-   */
+  /** {@return the default proportional gain} */
   public double getDefaultP() {
     return defaultP;
   }
 
-  /**
-   * Returns the current P value.
-   *
-   * @return The current P value.
-   */
+  /** {@return the current proportional gain} */
   public double getP() {
     return Preferences.getDouble(getKey() + "/kP", defaultP);
   }
 
   /**
-   * Sets the P value.
+   * Sets the proportional gain.
    *
-   * @param p The P value to set.
+   * @param p The proportional gain to set.
    */
   public void setP(double p) {
     controller.setP(p);
     Preferences.setDouble(getKey() + "/kP", p);
   }
 
-  /**
-   * Returns the default I value.
-   *
-   * @return The default I value.
-   */
+  /** {@return the default integral gain} */
   public double getDefaultI() {
     return defaultI;
   }
 
-  /**
-   * Returns the current I value.
-   *
-   * @return The current I value.
-   */
+  /** {@return the current integral gain} */
   public double getI() {
     return Preferences.getDouble(getKey() + "/kI", defaultI);
   }
 
   /**
-   * Sets the I value.
+   * Sets the integral gain.
    *
-   * @param i The I value to set.
+   * @param i The integral gain to set.
    */
   public void setI(double i) {
     controller.setI(i);
     Preferences.setDouble(getKey() + "/kI", i);
   }
 
-  /**
-   * Returns the default D value.
-   *
-   * @return The default D value.
-   */
+  /** {@return the default derivative gain} */
   public double getDefaultD() {
     return defaultD;
   }
 
-  /**
-   * Returns the current D value.
-   *
-   * @return The current D value.
-   */
+  /** {@return the current derivative gain} */
   public double getD() {
     return Preferences.getDouble(getKey() + "/kD", defaultD);
   }
 
   /**
-   * Sets the D value.
+   * Sets the derivative gain.
    *
-   * @param d The D value to set.
+   * @param d The derivative gain to set.
    */
   public void setD(double d) {
     controller.setD(d);
     Preferences.setDouble(getKey() + "/kD", d);
   }
 
   /**
-   * Returns whether the PID controller is at its setpoint.
+   * Sets the PID coefficients.
    *
-   * @return True if the PID controller is at its setpoint, false otherwise.
+   * @param p The proportional gain to set.
+   * @param i The integral gain to set.
+   * @param d The derivative gain to set.
    */
-  public boolean atSetpoint() {
-    return controller.atSetpoint();
+  public void setPID(double p, double i, double d) {
+    controller.setPID(p, i, d);
+    Preferences.setDouble(getKey() + "/kP", p);
+    Preferences.setDouble(getKey() + "/kI", i);
+    Preferences.setDouble(getKey() + "/kD", d);
+  }
+
+  /** {@return the maximum magnitude of the error to allow integral control} */
+  public double getIZone() {
+    return controller.getIZone();
   }
 
   /**
-   * Calculates the output of the PID controller for the given measurement.
+   * Sets the maximum magnitude of the error to allow integral control.
    *
-   * @param measurement The current measurement.
-   * @return The output of the PID controller.
+   * @param iZone The IZone to set.
    */
-  public double calculate(double measurement) {
-    return controller.calculate(measurement);
+  public void setIZone(double iZone) {
+    controller.setIZone(iZone);
   }
 
   /**
-   * Calculates the output of the PID controller for the given measurement and setpoint.
+   * Sets the minimum and maximum contributions of the integral term.
    *
-   * @param measurement The current measurement.
-   * @param setpoint The desired setpoint.
-   * @return The output of the PID controller.
+   * @param minimumIntegral The minimum integral value.
+   * @param maximumIntegral The maximum integral value.
    */
-  public double calculate(double measurement, double setpoint) {
-    return controller.calculate(measurement, setpoint);
+  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
+    controller.setIntegratorRange(minimumIntegral, maximumIntegral);
+  }
+
+  /** {@return the period of this controller} */
+  public double getPeriod() {
+    return controller.getPeriod();
   }
 
   /**
-   * Enables continuous input for the PID controller.
+   * Enables continuous input for this controller.
+   *
+   * <p>Rather than use the minimum and maximum input as constraints, the controller considers them
+   * to be the endpoints of a continuous range. This is useful for circular inputs, such as angles.
    *
    * @param minimumInput The minimum input value.
    * @param maximumInput The maximum input value.
@@ -191,27 +210,70 @@ public void enableContinuousInput(double minimumInput, double maximumInput) {
     controller.enableContinuousInput(minimumInput, maximumInput);
   }
 
-  /** Disables continuous input for the PID controller. */
+  /** Disables continuous input for this controller. */
   public void disableContinuousInput() {
     controller.disableContinuousInput();
   }
 
   /**
-   * Returns whether continuous input is enabled for the PID controller.
+   * Returns whether continuous input is enabled for this controller.
    *
-   * @return True if continuous input is enabled, false otherwise.
+   * @return Returns true if continuous input is enabled, false otherwise.
    */
   public boolean isContinuousInputEnabled() {
     return controller.isContinuousInputEnabled();
   }
 
-  /** Resets the PID controller. */
+  /** Resets this controller. */
   public void reset() {
     controller.reset();
   }
 
   /**
-   * Sets the setpoint for the PID controller.
+   * Sets the error which is considered tolerable for use with {@link #atSetpoint()}.
+   *
+   * @param errorTolerance The error which is tolerable.
+   */
+  public void setTolerance(double errorTolerance) {
+    controller.setTolerance(errorTolerance);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with {@link #atSetpoint()}.
+   *
+   * @param errorTolerance The error which is tolerable.
+   * @param errorDerivativeTolerance The error derivative which is tolerable.
+   */
+  public void setTolerance(double errorTolerance, double errorDerivativeTolerance) {
+    controller.setTolerance(errorTolerance, errorDerivativeTolerance);
+  }
+
+  /** {@return the error tolerance of this controller} */
+  public double getErrorTolerance() {
+    return controller.getErrorTolerance();
+  }
+
+  /** {@return the error derivative tolerance of this controller} */
+  public double getErrorDerivativeTolerance() {
+    return controller.getErrorDerivativeTolerance();
+  }
+
+  /** {@return the current setpoint of this controller} */
+  public double getSetpoint() {
+    return controller.getSetpoint();
+  }
+
+  /**
+   * Returns whether this controller is at its setpoint.
+   *
+   * @return Returns true if this controller is at its setpoint, false otherwise.
+   */
+  public boolean atSetpoint() {
+    return controller.atSetpoint();
+  }
+
+  /**
+   * Sets the setpoint for this controller.
    *
    * @param setpoint The setpoint to set.
    */
@@ -220,22 +282,39 @@ public void setSetpoint(double setpoint) {
   }
 
   /**
-   * Sets the tolerance for the PID controller.
+   * Calculates the output of this controller for the given measurement.
    *
-   * @param errorTolerance The error which is considered acceptable.
+   * @param measurement The current measurement.
+   * @return The output of this controller.
    */
-  public void setTolerance(double errorTolerance) {
-    controller.setTolerance(errorTolerance);
+  public double calculate(double measurement) {
+    return controller.calculate(measurement);
   }
 
   /**
-   * Sets the tolerance for the PID controller.
+   * Calculates the output of this controller for the given measurement and setpoint.
    *
-   * @param errorTolerance The error which is considered acceptable.
-   * @param errorDerivativeTolerance The error derivative which is considered acceptable.
+   * @param measurement The current measurement.
+   * @param setpoint The desired setpoint.
+   * @return The output of this controller.
    */
-  public void setTolerance(double errorTolerance, double errorDerivativeTolerance) {
-    controller.setTolerance(errorTolerance, errorDerivativeTolerance);
+  public double calculate(double measurement, double setpoint) {
+    return controller.calculate(measurement, setpoint);
+  }
+
+  /** {@return the current error of this controller} */
+  public double getError() {
+    return controller.getError();
+  }
+
+  /** {@return the current accumulated error used in the integral calculation of this controller} */
+  public double getAccumulatedError() {
+    return controller.getAccumulatedError();
+  }
+
+  /** {@return the current error derivative of this controller} */
+  public double getErrorDerivative() {
+    return controller.getErrorDerivative();
   }
 
   @Override