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

wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp

@@ -18,10 +18,10 @@ ArmSubsystem::ArmSubsystem()
               kP, 0, 0, {kMaxVelocity, kMaxAcceleration})),
       m_motor(kMotorPort),
       m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
-      m_feedforward(kS, kCos, kV, kA),
-      // Start arm at rest in neutral position
-      m_goal{kArmOffset, 0_rad_per_s} {
+      m_feedforward(kS, kCos, kV, kA) {
   m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.to<double>());
+  // Start arm in neutral position
+  SetGoal(State{kArmOffset, 0_rad_per_s});
 }
 
 void ArmSubsystem::UseOutput(double output, State setpoint) {
@@ -32,12 +32,6 @@ void ArmSubsystem::UseOutput(double output, State setpoint) {
   m_motor.SetVoltage(units::volt_t(output) + feedforward);
 }
 
-void ArmSubsystem::SetGoal(units::radian_t goal) {
-  m_goal = State{goal, 0_rad_per_s};
-}
-
-State ArmSubsystem::GetGoal() { return m_goal; }
-
 units::radian_t ArmSubsystem::GetMeasurement() {
   return units::radian_t(m_encoder.GetDistance()) + kArmOffset;
 }

wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h

@@ -24,16 +24,10 @@ class ArmSubsystem : public frc2::ProfiledPIDSubsystem<units::radians> {
 
   void UseOutput(double output, State setpoint) override;
 
-  void SetGoal(units::radian_t goal);
-
-  State GetGoal() override;
-
   units::radian_t GetMeasurement() override;
 
  private:
   frc::PWMVictorSPX m_motor;
   frc::Encoder m_encoder;
   frc::ArmFeedforward m_feedforward;
-
-  State m_goal;
 };

wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp

@@ -17,32 +17,22 @@ ShooterSubsystem::ShooterSubsystem()
     : PIDSubsystem(frc2::PIDController(kP, kI, kD)),
       m_shooterMotor(kShooterMotorPort),
       m_feederMotor(kFeederMotorPort),
-      m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]) {
+      m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]),
+      m_shooterFeedforward(kS, kV) {
   m_controller.SetTolerance(kShooterToleranceRPS.to<double>());
   m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  SetSetpoint(kShooterTargetRPS.to<double>());
 }
 
-void ShooterSubsystem::UseOutput(double output) {
-  // Use a feedforward of the form kS + kV * velocity
-  m_shooterMotor.SetVoltage(units::volt_t(output) + kS +
-                            kV * kShooterTargetRPS);
-}
-
-void ShooterSubsystem::Disable() {
-  // Turn off motor when we disable, since useOutput(0) doesn't stop the motor
-  // due to our feedforward
-  frc2::PIDSubsystem::Disable();
-  m_shooterMotor.Set(0);
+void ShooterSubsystem::UseOutput(double output, double setpoint) {
+  m_shooterMotor.SetVoltage(units::volt_t(output) +
+                            m_shooterFeedforward.Calculate(kShooterTargetRPS));
 }
 
 bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); }
 
 double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); }
 
-double ShooterSubsystem::GetSetpoint() {
-  return kShooterTargetRPS.to<double>();
-}
-
 void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); }
 
 void ShooterSubsystem::StopFeeder() { m_feederMotor.Set(0); }

wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h

@@ -9,20 +9,18 @@
 
 #include <frc/Encoder.h>
 #include <frc/PWMVictorSPX.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc2/command/PIDSubsystem.h>
+#include <units/units.h>
 
 class ShooterSubsystem : public frc2::PIDSubsystem {
  public:
   ShooterSubsystem();
 
-  void UseOutput(double output) override;
-
-  double GetSetpoint() override;
+  void UseOutput(double output, double setpoint) override;
 
   double GetMeasurement() override;
 
-  void Disable() override;
-
   bool AtSetpoint();
 
   void RunFeeder();
@@ -33,4 +31,5 @@ class ShooterSubsystem : public frc2::PIDSubsystem {
   frc::PWMVictorSPX m_shooterMotor;
   frc::PWMVictorSPX m_feederMotor;
   frc::Encoder m_shooterEncoder;
+  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward;
 };

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp

@@ -28,8 +28,6 @@ void Elevator::Log() { frc::SmartDashboard::PutData("Wrist Pot", &m_pot); }
 
 double Elevator::GetMeasurement() { return m_pot.Get(); }
 
-void Elevator::UseOutput(double d) { m_motor.Set(d); }
-
-double Elevator::GetSetpoint() { return m_setpoint; }
-
-void Elevator::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+void Elevator::UseOutput(double output, double setpoint) {
+  m_motor.Set(output);
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp

@@ -26,10 +26,6 @@ void Wrist::Log() {
   // frc::SmartDashboard::PutData("Wrist Angle", &m_pot);
 }
 
-double Wrist::GetSetpoint() { return m_setpoint; }
-
-void Wrist::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
-
 double Wrist::GetMeasurement() { return m_pot.Get(); }
 
-void Wrist::UseOutput(double d) { m_motor.Set(d); }
+void Wrist::UseOutput(double output, double setpoint) { m_motor.Set(output); }

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h

@@ -37,14 +37,7 @@ class Elevator : public frc2::PIDSubsystem {
    * by
    * the subsystem.
    */
-  void UseOutput(double d) override;
-
-  double GetSetpoint() override;
-
-  /**
-   * Sets the setpoint for the subsystem.
-   */
-  void SetSetpoint(double setpoint);
+  void UseOutput(double output, double setpoint) override;
 
  private:
   frc::PWMVictorSPX m_motor{5};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h

@@ -35,14 +35,7 @@ class Wrist : public frc2::PIDSubsystem {
    * by
    * the subsystem.
    */
-  void UseOutput(double d) override;
-
-  double GetSetpoint() override;
-
-  /**
-   * Sets the setpoint for the subsystem.
-   */
-  void SetSetpoint(double setpoint);
+  void UseOutput(double output, double setpoint) override;
 
  private:
   frc::PWMVictorSPX m_motor{6};

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java

@@ -35,8 +35,6 @@ public class ArmSubsystem extends ProfiledPIDSubsystem {
   private final ArmFeedforward m_feedforward =
       new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad);
 
-  private TrapezoidProfile.State m_goal;
-
   /**
    * Create a new ArmSubsystem.
    */
@@ -49,7 +47,7 @@ public class ArmSubsystem extends ProfiledPIDSubsystem {
                                          kMaxAccelerationRadPerSecSquared)));
     m_encoder.setDistancePerPulse(kEncoderDistancePerPulse);
     // Start arm at rest in neutral position
-    m_goal = new TrapezoidProfile.State(kArmOffsetRads, 0);
+    setGoal(kArmOffsetRads);
   }
 
   @Override
@@ -60,20 +58,6 @@ public class ArmSubsystem extends ProfiledPIDSubsystem {
     m_motor.setVoltage(output + feedforward);
   }
 
-  /**
-   * Sets the goal position for the arm.
-   *
-   * @param position The goal position, in radians.
-   */
-  public void setGoal(double position) {
-    m_goal = new TrapezoidProfile.State(position, 0);
-  }
-
-  @Override
-  public TrapezoidProfile.State getGoal() {
-    return m_goal;
-  }
-
   @Override
   public double getMeasurement() {
     return m_encoder.getDistance() + kArmOffsetRads;

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java

@@ -10,6 +10,7 @@ package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj2.command.PIDSubsystem;
 
 import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kD;
@@ -31,6 +32,8 @@ public class ShooterSubsystem extends PIDSubsystem {
   private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(kFeederMotorPort);
   private final Encoder m_shooterEncoder =
       new Encoder(kEncoderPorts[0], kEncoderPorts[1], kEncoderReversed);
+  private final SimpleMotorFeedforward m_shooterFeedforward
+      = new SimpleMotorFeedforward(kSVolts, kVVoltSecondsPerRotation);
 
   /**
    * The shooter subsystem for the robot.
@@ -39,17 +42,12 @@ public class ShooterSubsystem extends PIDSubsystem {
     super(new PIDController(kP, kI, kD));
     getController().setTolerance(kShooterToleranceRPS);
     m_shooterEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+    setSetpoint(kShooterTargetRPS);
   }
 
   @Override
-  public void useOutput(double output) {
-    // Use a feedforward of the form kS + kV * velocity
-    m_shooterMotor.setVoltage(output + kSVolts + kVVoltSecondsPerRotation * kShooterTargetRPS);
-  }
-
-  @Override
-  public double getSetpoint() {
-    return kShooterTargetRPS;
+  public void useOutput(double output, double setpoint) {
+    m_shooterMotor.setVoltage(output + m_shooterFeedforward.calculate(setpoint));
   }
 
   @Override
@@ -68,12 +66,4 @@ public class ShooterSubsystem extends PIDSubsystem {
   public void stopFeeder() {
     m_feederMotor.set(0);
   }
-
-  @Override
-  public void disable() {
-    super.disable();
-    // Turn off motor when we disable, since useOutput(0) doesn't stop the motor due to our
-    // feedforward
-    m_shooterMotor.set(0);
-  }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java

@@ -22,7 +22,6 @@ import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
 public class Elevator extends PIDSubsystem {
   private final Victor m_motor;
   private final AnalogPotentiometer m_pot;
-  private double m_setpoint;
 
   private static final double kP_real = 4;
   private static final double kI_real = 0.07;
@@ -73,26 +72,7 @@ public class Elevator extends PIDSubsystem {
    * Use the motor as the PID output. This method is automatically called by the subsystem.
    */
   @Override
-  public void useOutput(double output) {
+  public void useOutput(double output, double setpoint) {
     m_motor.set(output);
   }
-
-  /**
-   * Returns the setpoint used by the PIDController.
-   *
-   * @return The setpoint for the PIDController.
-   */
-  @Override
-  public double getSetpoint() {
-    return m_setpoint;
-  }
-
-  /**
-   * Sets the setpoint used by the PIDController.
-   *
-   * @param setpoint The setpoint for the PIDController.
-   */
-  public void setSetpoint(double setpoint) {
-    m_setpoint = setpoint;
-  }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java

@@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
 public class Wrist extends PIDSubsystem {
   private final Victor m_motor;
   private final AnalogPotentiometer m_pot;
-  private double m_setpoint;
 
   private static final double kP_real = 1;
   private static final double kP_simulation = 0.05;
@@ -70,26 +69,7 @@ public class Wrist extends PIDSubsystem {
    * Use the motor as the PID output. This method is automatically called by the subsystem.
    */
   @Override
-  public void useOutput(double output) {
+  public void useOutput(double output, double setpoint) {
     m_motor.set(output);
   }
-
-  /**
-   * Returns the setpoint used by the PIDController.
-   *
-   * @return The setpoint for the PIDController.
-   */
-  @Override
-  public double getSetpoint() {
-    return m_setpoint;
-  }
-
-  /**
-   * Sets the setpoint used by the PIDController.
-   *
-   * @param setpoint The setpoint for the PIDController.
-   */
-  public void setSetpoint(double setpoint) {
-    m_setpoint = setpoint;
-  }
 }