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.

wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp

@@ -14,18 +14,22 @@ PIDSubsystem::PIDSubsystem(PIDController controller)
 
 void PIDSubsystem::Periodic() {
   if (m_enabled) {
-    UseOutput(m_controller.Calculate(GetMeasurement(), GetSetpoint()));
+    UseOutput(m_controller.Calculate(GetMeasurement(), m_setpoint), m_setpoint);
   }
 }
 
+void PIDSubsystem::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+
 void PIDSubsystem::Enable() {
   m_controller.Reset();
   m_enabled = true;
 }
 
 void PIDSubsystem::Disable() {
-  UseOutput(0);
+  UseOutput(0, 0);
   m_enabled = false;
 }
 
+bool PIDSubsystem::IsEnabled() { return m_enabled; }
+
 PIDController& PIDSubsystem::GetController() { return m_controller; }

wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h

@@ -30,25 +30,11 @@ class PIDSubsystem : public SubsystemBase {
   void Periodic() override;
 
   /**
-   * Uses the output from the PIDController.
+   * Sets the setpoint for the subsystem.
    *
-   * @param output the output of the PIDController
+   * @param setpoint the setpoint for the subsystem
    */
-  virtual void UseOutput(double output) = 0;
-
-  /**
-   * Returns the reference (setpoint) used by the PIDController.
-   *
-   * @return the reference (setpoint) to be used by the controller
-   */
-  virtual double GetSetpoint() = 0;
-
-  /**
-   * Returns the measurement of the process variable used by the PIDController.
-   *
-   * @return the measurement of the process variable
-   */
-  virtual double GetMeasurement() = 0;
+  void SetSetpoint(double setpoint);
 
   /**
    * Enables the PID control.  Resets the controller.
@@ -60,6 +46,13 @@ class PIDSubsystem : public SubsystemBase {
    */
   virtual void Disable();
 
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  bool IsEnabled();
+
   /**
    * Returns the PIDController.
    *
@@ -70,5 +63,23 @@ class PIDSubsystem : public SubsystemBase {
  protected:
   PIDController m_controller;
   bool m_enabled;
+
+  /**
+   * Returns the measurement of the process variable used by the PIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  virtual double GetMeasurement() = 0;
+
+  /**
+   * Uses the output from the PIDController.
+   *
+   * @param output the output of the PIDController
+   * @param setpoint the setpoint of the PIDController (for feedforward)
+   */
+  virtual void UseOutput(double output, double setpoint) = 0;
+
+ private:
+  double m_setpoint{0};
 };
 }  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h

@@ -38,34 +38,24 @@ class ProfiledPIDSubsystem : public SubsystemBase {
 
   void Periodic() override {
     if (m_enabled) {
-      UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
+      UseOutput(m_controller.Calculate(GetMeasurement(), m_goal),
                 m_controller.GetSetpoint());
     }
   }
 
   /**
-   * Uses the output from the ProfiledPIDController.
+   * Sets the goal state for the subsystem.
    *
-   * @param output the output of the ProfiledPIDController
-   * @param setpoint the setpoint state of the ProfiledPIDController, for
-   * feedforward
+   * @param goal The goal state for the subsystem's motion profile.
    */
-  virtual void UseOutput(double output, State setpoint) = 0;
+  void SetGoal(State goal) { m_goal = goal; }
 
   /**
-   * Returns the goal used by the ProfiledPIDController.
+   * Sets the goal state for the subsystem.  Goal velocity assumed to be zero.
    *
-   * @return the goal to be used by the controller
+   * @param goal The goal position for the subsystem's motion profile.
    */
-  virtual State GetGoal() = 0;
-
-  /**
-   * Returns the measurement of the process variable used by the
-   * ProfiledPIDController.
-   *
-   * @return the measurement of the process variable
-   */
-  virtual Distance_t GetMeasurement() = 0;
+  void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
 
   /**
    * Enables the PID control.  Resets the controller.
@@ -83,6 +73,13 @@ class ProfiledPIDSubsystem : public SubsystemBase {
     m_enabled = false;
   }
 
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  bool IsEnabled() { return m_enabled; }
+
   /**
    * Returns the ProfiledPIDController.
    *
@@ -92,6 +89,26 @@ class ProfiledPIDSubsystem : public SubsystemBase {
 
  protected:
   frc::ProfiledPIDController<Distance> m_controller;
-  bool m_enabled;
+  bool m_enabled{false};
+
+  /**
+   * Returns the measurement of the process variable used by the
+   * ProfiledPIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  virtual Distance_t GetMeasurement() = 0;
+
+  /**
+   * Uses the output from the ProfiledPIDController.
+   *
+   * @param output the output of the ProfiledPIDController
+   * @param setpoint the setpoint state of the ProfiledPIDController, for
+   * feedforward
+   */
+  virtual void UseOutput(double output, State setpoint) = 0;
+
+ private:
+  State m_goal;
 };
 }  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h

@@ -41,22 +41,29 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
                             units::second_t period = 20_ms)
       : m_constraints(constraints),
         m_state{position, Velocity_t(0)},
+        m_goal{position, Velocity_t{0}},
         m_period(period) {}
 
   void Periodic() override {
     auto profile =
-        frc::TrapezoidProfile<Distance>(m_constraints, GetGoal(), m_state);
+        frc::TrapezoidProfile<Distance>(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.
    */
-  virtual State GetGoal() = 0;
+  void SetGoal(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.
+   */
+  void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
 
  protected:
   /**
@@ -70,6 +77,7 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
  private:
   Constraints m_constraints;
   State m_state;
+  State m_goal;
   units::second_t m_period;
 };
 }  // namespace frc2