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.

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};