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.

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