Improve various subsystem APIs (#2130)

Improves the APIs for various prebuilt subsystems (PIDSubsystem, TrapezoidProfileSubsystem, ProfiledPIDSubsystem). Addresses #2128, and also changes the rather cumbersome getSetpoint API to a more intuitive setSetpoint one. Updates examples to match.

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java

@@ -19,6 +19,8 @@ public abstract class PIDSubsystem extends SubsystemBase {
   protected final PIDController m_controller;
   protected boolean m_enabled;
 
+  private double m_setpoint;
+
   /**
    * Creates a new PIDSubsystem.
    *
@@ -32,7 +34,7 @@ public abstract class PIDSubsystem extends SubsystemBase {
   @Override
   public void periodic() {
     if (m_enabled) {
-      useOutput(m_controller.calculate(getMeasurement(), getSetpoint()));
+      useOutput(m_controller.calculate(getMeasurement(), m_setpoint), m_setpoint);
     }
   }
 
@@ -40,26 +42,29 @@ public abstract class PIDSubsystem extends SubsystemBase {
     return m_controller;
   }
 
+  /**
+   * Sets the setpoint for the subsystem.
+   *
+   * @param setpoint the setpoint for the subsystem
+   */
+  public void setSetpoint(double setpoint) {
+    m_setpoint = setpoint;
+  }
+
   /**
    * Uses the output from the PIDController.
    *
    * @param output the output of the PIDController
+   * @param setpoint the setpoint of the PIDController (for feedforward)
    */
-  public abstract void useOutput(double output);
-
-  /**
-   * Returns the reference (setpoint) used by the PIDController.
-   *
-   * @return the reference (setpoint) to be used by the controller
-   */
-  public abstract double getSetpoint();
+  protected abstract void useOutput(double output, double setpoint);
 
   /**
    * Returns the measurement of the process variable used by the PIDController.
    *
    * @return the measurement of the process variable
    */
-  public abstract double getMeasurement();
+  protected abstract double getMeasurement();
 
   /**
    * Enables the PID control.  Resets the controller.
@@ -74,6 +79,15 @@ public abstract class PIDSubsystem extends SubsystemBase {
    */
   public void disable() {
     m_enabled = false;
-    useOutput(0);
+    useOutput(0, 0);
+  }
+
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  public boolean isEnabled() {
+    return m_enabled;
   }
 }

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java

@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj2.command;
 
 import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
 
 import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
@@ -20,6 +21,8 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
   protected final ProfiledPIDController m_controller;
   protected boolean m_enabled;
 
+  private TrapezoidProfile.State m_goal;
+
   /**
    * Creates a new ProfiledPIDSubsystem.
    *
@@ -33,7 +36,7 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
   @Override
   public void periodic() {
     if (m_enabled) {
-      useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint());
+      useOutput(m_controller.calculate(getMeasurement(), m_goal), m_controller.getSetpoint());
     }
   }
 
@@ -41,27 +44,38 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
     return m_controller;
   }
 
+  /**
+   * Sets the goal state for the subsystem.
+   *
+   * @param goal The goal state for the subsystem's motion profile.
+   */
+  public void setGoal(TrapezoidProfile.State goal) {
+    m_goal = goal;
+  }
+
+  /**
+   * Sets the goal state for the subsystem.  Goal velocity assumed to be zero.
+   *
+   * @param goal The goal position for the subsystem's motion profile.
+   */
+  public void setGoal(double goal) {
+    setGoal(new TrapezoidProfile.State(goal, 0));
+  }
+
   /**
    * Uses the output from the ProfiledPIDController.
    *
    * @param output   the output of the ProfiledPIDController
    * @param setpoint the setpoint state of the ProfiledPIDController, for feedforward
    */
-  public abstract void useOutput(double output, State setpoint);
-
-  /**
-   * Returns the goal used by the ProfiledPIDController.
-   *
-   * @return the goal to be used by the controller
-   */
-  public abstract State getGoal();
+  protected abstract void useOutput(double output, State setpoint);
 
   /**
    * Returns the measurement of the process variable used by the ProfiledPIDController.
    *
    * @return the measurement of the process variable
    */
-  public abstract double getMeasurement();
+  protected abstract double getMeasurement();
 
   /**
    * Enables the PID control.  Resets the controller.
@@ -78,4 +92,13 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
     m_enabled = false;
     useOutput(0, new State());
   }
+
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  public boolean isEnabled() {
+    return m_enabled;
+  }
 }

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java

@@ -18,6 +18,7 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
   private final TrapezoidProfile.Constraints m_constraints;
 
   private TrapezoidProfile.State m_state;
+  private TrapezoidProfile.State m_goal;
 
   /**
    * Creates a new TrapezoidProfileSubsystem.
@@ -51,17 +52,28 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
 
   @Override
   public void periodic() {
-    var profile = new TrapezoidProfile(m_constraints, getGoal(), m_state);
+    var profile = new TrapezoidProfile(m_constraints, m_goal, m_state);
     m_state = profile.calculate(m_period);
     useState(m_state);
   }
 
   /**
-   * Users should override this to return the goal state for the subsystem's motion profile.
+   * Sets the goal state for the subsystem.
    *
-   * @return The goal state for the subsystem's motion profile.
+   * @param goal The goal state for the subsystem's motion profile.
    */
-  public abstract TrapezoidProfile.State getGoal();
+  public void setGoal(TrapezoidProfile.State goal) {
+    m_goal = goal;
+  }
+
+  /**
+   * Sets the goal state for the subsystem.  Goal velocity assumed to be zero.
+   *
+   * @param goal The goal position for the subsystem's motion profile.
+   */
+  public void setGoal(double goal) {
+    setGoal(new TrapezoidProfile.State(goal, 0));
+  }
 
   /**
    * Users should override this to consume the current state of the motion profile.