Unit 8 · Lesson 7

WPILib `PIDController`

In Lesson 6 you built PID from scratch — three state fields, a differential, an accumulator, a clamp. Now you replace all of it with one WPILib class. PIDController does everything the manual implementation did, and then adds derivative-on-measurement, continuous input for angles, built-in integrator clamping, tolerance detection, and a clean reset. This is the production-standard tool for every position and velocity PID loop on an FRC robot.

By the end of this lesson, you will:

Construct a PIDController and call calculate(measurement) in a subsystem's periodic method
Configure tolerance with setTolerance() and use atSetpoint() in a command's isFinished()
Enable continuous input with enableContinuousInput() and explain exactly what it changes for angular mechanisms
Prevent integral windup using setIntegratorRange() and setIZone()
Explain why derivative-on-measurement eliminates derivative kick that manual PID suffers
Implement a complete arm position command using PIDController with correct initialize() / execute() / isFinished() structure

From Manual to PIDController: The Diff

Here is the complete transformation of the Lesson 6 manual PID implementation into the WPILib class. Every manual state field, every loop-end update, and every manual clamp disappears and is replaced by the library. The result is fewer lines, fewer bugs, and better behavior.

ArmSubsystem.java — manual PID → WPILib PIDController

// ── Fields ────────────────────────────────────────────
−private double m_prevError = 0.0;
−private double m_integralSum = 0.0;
−private static final double kP = 0.05, kI = 0.002, kD = 0.002, kIMax = 2.0;
+private final PIDController m_pid = new PIDController(0.05, 0.002, 0.002);

// ── Constructor (one-time configuration) ──────────────
+// in constructor:
+m_pid.setTolerance(1.0, 5.0); // ±1° position, ±5°/s velocity
+m_pid.setIntegratorRange(-2.0, 2.0); // ±2V max I output

// ── In moveToAngle() ──────────────────────────────────
−double error = setpointDeg - currentDeg;
−double pOut = kP * error;
−m_integralSum += error * 0.020;
−m_integralSum = MathUtil.clamp(m_integralSum, -kIMax/kI, kIMax/kI);
−double iOut = kI * m_integralSum;
−double errorRate = (error - m_prevError) / 0.020;
−double dOut = kD * errorRate;
−m_prevError = error;
−double output = MathUtil.clamp(pOut+iOut+dOut, -12, 12);
+m_pid.setSetpoint(setpointDeg);
+double output = MathUtil.clamp(m_pid.calculate(currentDeg), -12, 12);

Twelve lines of hand-written control code collapses to two. The behavior is better in every measurable way — but the most important improvement isn't visible in this diff. It's in how the D term is computed.

The Key Improvement: Derivative on Measurement

The manual D term in Lesson 6 computed the derivative of the error: (error − prevError) / dt. When the setpoint suddenly changes — say, from 45° to 90° — error jumps from near-zero to 45° in a single 20 ms cycle. The derivative term sees (45 − 0) / 0.020 = 2250°/s and outputs an enormous voltage spike. This is derivative kick.

WPILib's PIDController instead computes the derivative of the measurement: (currentMeasurement − prevMeasurement) / dt. When you change the setpoint, the measurement doesn't jump — only the mechanism moving changes the measurement. The D term only responds to the mechanism's actual motion, not to setpoint changes. No spike. No kick. The transition from one setpoint to another is smooth.

💡 This is why you pass the measurement to calculate(), not the error

m_pid.calculate(measurement) — not m_pid.calculate(error). The PIDController computes error internally as setpoint − measurement and uses the measurement directly for the derivative. Passing the error instead of the measurement would make the D term compute the derivative of error (undoing the derivative-on-measurement improvement) and would make the integral accumulate the wrong value. Pass measurement. Always.

Complete API Reference

Every public method you'll use. The Essential tag marks methods called in every implementation; Configuration marks one-time setup in the constructor; Diagnostic marks telemetry and debugging helpers.

Method	What it does
new PIDController(kP, kI, kD)essential	Constructs with gains. Default period is 0.020s (50 Hz). Use the four-argument constructor `PIDController(kP, kI, kD, period)` only if your loop runs at a non-standard rate.
calculate(measurement)essential	The main call. Pass the current sensor reading. Returns the controller output. Call every loop cycle at the same rate as the period argument. Internally computes error = setpoint − measurement, updates P/I/D, returns sum.
calculate(measurement, setpoint)essential	Overload: sets the setpoint and computes output in one call. Equivalent to calling `setSetpoint(setpoint)` then `calculate(measurement)`. Most concise pattern for commands that have a fixed setpoint per execute() call.
setSetpoint(setpoint)config	Sets the target value. Can be called before calculate() each loop or once in the command's initialize(). The integral resets when the setpoint changes by a significant amount (configurable).
setTolerance(posTol, velTol)config	Sets the position and velocity tolerances for `atSetpoint()`. Both conditions must be satisfied simultaneously. Position tolerance in measurement units; velocity tolerance in measurement-units per second. Call once in constructor.
atSetpoint()essential	Returns true when both \|error\| ≤ position tolerance AND \|velocity error\| ≤ velocity tolerance (if set). Use in command's `isFinished()`. Never returns true if tolerance hasn't been set (defaults to infinity).
enableContinuousInput(min, max)config	Makes the input space circular between min and max. Essential for angles, steering, and any wrapping sensor. Causes the controller to always take the shortest path around the wrap boundary. Call once in constructor for angular mechanisms.
setIntegratorRange(min, max)config	Clamps the integral accumulator's output contribution to [min, max] volts. Built-in anti-windup. Call with your desired maximum I-term contribution (e.g., `−2.0, 2.0`). Replaces the manual kIMax/kI clamp from Lesson 6.
setIZone(iZone)config	The integral only accumulates when \|error\| ≤ iZone. Prevents windup during large transients (mechanism far from setpoint). Set to roughly 5–20% of your typical operating error range.
reset()essential	Clears the integral accumulator and the previous-measurement value used for derivative. Call from command's `initialize()` before each use.
getError()diagnostic	Returns current error (setpoint − measurement). Publish to SmartDashboard during tuning.
getSetpoint()diagnostic	Returns the currently configured setpoint. Useful for verifying the correct target is set.
getP() / getI() / getD()diagnostic	Returns the current gains. Useful when gains are changed at runtime (e.g., from SmartDashboard during tuning).
setPID(kP, kI, kD)	Updates all three gains at once. Useful for runtime tuning via NetworkTables or SmartDashboard sliders during development.

Continuous Input: The Most Important Configuration for Angles

Without continuous input, a PID controller sees angles as a number line — not a circle. A steering motor at 350° commanded to go to 10° has an error of 10 − 350 = −340°. The controller drives 340° counterclockwise. The correct move is 20° clockwise. enableContinuousInput(−180, 180) tells the controller the space is circular: error is computed as the shortest angular distance around the wrap boundary.

Continuous input — steering from 350° to 10°

Without enableContinuousInput

With enableContinuousInput(−180, 180)

Select an angle transition above to see how continuous input changes the controller's path.

⚠️ enableContinuousInput requires consistent units

The min and max arguments to enableContinuousInput() must match the units of the measurement and setpoint passed to calculate(). If you measure in degrees (−180 to 180), use enableContinuousInput(−180, 180). If you measure in radians (−π to π), use enableContinuousInput(−Math.PI, Math.PI). A common mistake: measuring in radians but setting continuous input to (−180, 180). The controller then wraps at ±180 radians — almost 30 full rotations — and produces enormous errors.

Complete Integration Pattern

Click each tab to see the full subsystem and command pattern. Click highlighted tokens for explanations.

ArmSubsystem.java + MoveArmToAngleCommand.java

public class ArmSubsystem extends SubsystemBase {

  private final TalonFX m_motor = new TalonFX(Constants.Arm.kMotorId);

  private final PIDController m_pid =
    new PIDController(
      Constants.Arm.kP,
      Constants.Arm.kI,
      Constants.Arm.kD);

  private final StatusSignal<Double> m_posSignal =
    m_motor.getPosition();

  public ArmSubsystem() {
    // One-time PID configuration in constructor
    m_pid.setTolerance(1.0, 5.0);       // ±1° pos, ±5°/s vel
    m_pid.setIntegratorRange(-1.5, 1.5); // ±1.5V max I
    m_pid.setIZone(5.0);               // I only within 5°
  }

  /** Called by MoveArmToAngleCommand.execute() every 20 ms. */
  public void driveToAngle(double setpointDeg) {
    double measurement = getAngleDegrees();
    double pidOut = m_pid.calculate(measurement, setpointDeg);
    m_motor.setVoltage(MathUtil.clamp(pidOut, -12, 12));
  }

  /** Returns true when arm is settled at setpoint. */
  public boolean isAtTarget() {
    return m_pid.atSetpoint();
  }

  public void resetPID() {
    m_pid.reset();
  }

  public double getAngleDegrees() {
    return m_posSignal.refresh().getValueAsDouble()
      / Constants.Arm.kGearRatio * 360.0;
  }

  @Override
  public void periodic() {
    SmartDashboard.putNumber("Arm/AngleDeg", getAngleDegrees());
    SmartDashboard.putNumber("Arm/Error", m_pid.getError());
    SmartDashboard.putBoolean("Arm/AtTarget", m_pid.atSetpoint());
  }
}

← click a highlighted token

public class MoveArmToAngleCommand extends CommandBase {

  private final ArmSubsystem m_arm;
  private final double m_targetDeg;

  public MoveArmToAngleCommand(ArmSubsystem arm, double targetDeg) {
    m_arm = arm;
    m_targetDeg = targetDeg;
    addRequirements(arm);
  }

  @Override
  public void initialize() {
    m_arm.resetPID(); // always reset before first execute()
  }

  @Override
  public void execute() {
    m_arm.driveToAngle(m_targetDeg);
  }

  @Override
  public boolean isFinished() {
    return m_arm.isAtTarget();
  }

  @Override
  public void end(boolean interrupted) {
    if (!interrupted) {
      // Hold at target using gravity compensation only
      m_arm.holdPosition();
    } else {
      m_arm.stop();
    }
  }
}

← click a highlighted token

The Four Most Common PIDController Mistakes

⚠️ Mistake 1: Passing error to calculate() instead of measurement

m_pid.calculate(error) is wrong. m_pid.calculate(currentAngle) is correct. The PIDController computes error internally. If you pass error, the controller computes error-of-error — a meaningless second derivative — and the D term computes the derivative of error (re-introducing derivative kick). Always pass the raw sensor reading, not the pre-computed error.

⚠️ Mistake 2: Not calling reset() at the start of each command

The PIDController retains its integral accumulator and previous-measurement state between uses. If a command finishes, then the same subsystem command runs again — without a reset — the I term carries over from the previous run. If the arm overshot last time, the integral may be negative, causing the new command to initially drive in the wrong direction. Call m_arm.resetPID() in initialize(). Always.

⚠️ Mistake 3: Using setTolerance() but forgetting the velocity tolerance

setTolerance(positionTol) sets only the position tolerance — velocity tolerance defaults to infinity (always satisfied). This means atSetpoint() returns true as soon as the arm is within the position band, even if it's moving quickly through the band. Result: the command finishes while the arm is still moving, and it overshoots after the command ends. Always set both: setTolerance(positionTol, velocityTol). The velocity tolerance prevents early exits during fast passes through the setpoint.

⚠️ Mistake 4: enableContinuousInput with wrong units or range

enableContinuousInput(min, max) must match the measurement units exactly. Measuring in radians but passing degrees to continuous input (or vice versa) makes the controller wrap at the wrong boundary, producing 180× or 57× the correct error. Check: if your mechanism measures in degrees 0–360, use enableContinuousInput(0, 360) or convert to −180 to 180 and use enableContinuousInput(−180, 180). If measuring in radians, use enableContinuousInput(−Math.PI, Math.PI).

🔍 LRI Perspective: "atSetpoint() requires both tolerances, or it fires too early"

Every season I watch at least one team scratch their heads because their arm command finishes early — the arm reaches its target, starts the next command, and then keeps moving from residual momentum. They check the setpoint. They check the encoder. Everything looks right. Then I ask: "Did you set a velocity tolerance?" The answer is always no. setTolerance(2.0) with no velocity tolerance means atSetpoint() fires the moment the arm swings through 88°–90°, even at full speed. It's inside the band for one 20 ms loop and the command exits. The fix is one number: setTolerance(2.0, 10.0). Position within 2°, and moving slower than 10°/s. Both must be true simultaneously before the command is done.

🔌 System Check — PIDController Bringup Sequence

After replacing manual PID with PIDController:

Publish m_pid.getError(), m_pid.atSetpoint(), and the output voltage to SmartDashboard. Command the arm to 90°. Confirm error starts large and decreases to within tolerance. Confirm atSetpoint() only becomes true when both position and velocity are within their tolerance bands — not during the swing.
Test the reset: command to 45°, let it settle, immediately command to 90° without resetting. Observe any initial wrong-direction behavior from a wound-up integral. Then add the m_arm.resetPID() call in initialize() and repeat. The initial wrong-direction behavior should disappear.
If the mechanism is angular (swerve steering, turret), verify enableContinuousInput() by commanding across the wrap boundary. Without it: 350° → 10° takes 340° the long way. With it: 20° the short way. Test this explicitly before competition.
Verify setIntegratorRange() is active: hold the mechanism against a hard stop for 10 seconds. Release. The I output should be bounded — no large spike. If there is a spike, the integrator range is not set or the range is too wide.

Knowledge Check

1. A swerve steering module reads −170° and needs to reach +170°. Without enableContinuousInput(−180, 180), how many degrees does the controller rotate the wheel, and in which direction?

A 20° clockwise — the shortest path across the ±180° boundary.
B 340° counterclockwise — error = 170 − (−170) = +340°. The controller drives a full 340° rotation in the positive direction instead of the optimal 20° crossing the wrap boundary. This produces visible lag in swerve steering and, at high drive speeds, causes the wheel to spin nearly all the way around on every direction reversal.
C 0° — the controller detects the wrap boundary and stops.
D 180° — the controller always takes the midpoint.

2. Your arm command's isFinished() calls m_arm.isAtTarget(), which calls m_pid.atSetpoint(). You set setTolerance(2.0) (no velocity tolerance). The arm is swinging at 15°/s when it passes through 89° on its way to 90°. Does the command finish? Should it?

A No — atSetpoint() requires the mechanism to be stationary before returning true.
B Yes, it finishes — and it shouldn't. Without a velocity tolerance, atSetpoint() returns true whenever |error| ≤ 2.0, regardless of speed. At 89° with a 90° setpoint, |error| = 1.0° ≤ 2.0°, so the command exits while the arm is still moving at 15°/s. The arm then coasts to some unknown position past the setpoint. Fix: setTolerance(2.0, 5.0) — position within 2° AND moving slower than 5°/s.
C Yes, it finishes — and this is correct behavior. The arm will decelerate and hold on its own.
D No — atSetpoint() requires at least 5 consecutive loops within tolerance before returning true.

3. You call m_pid.calculate(m_pid.getError()) instead of m_pid.calculate(getAngleDegrees()). The setpoint is 90° and the current angle is 85°. What value does the controller actually compute error as internally?

A 5° — same as the correct implementation, since getError() returns the right value.
B 85° — internal error = setpoint − measurement = 90 − 5 = 85°. You passed 5 (the error) as the "measurement", so the controller treats 5 as the current position and computes error = 90 − 5 = 85°, which is wrong. The controller then commands voltage for an 85° error instead of a 5° error — massive output for a small real error.
C 0° — the controller detects that you passed a computed error and adjusts.
D −85° — the sign is inverted because error and measurement are both used for the subtraction.

💪 Practice Prompt

Migrate to WPILib PIDController

Take the manual PID from Lesson 6 and apply the diff from the top of this lesson. Replace the three state fields and the nine calculation lines with a single PIDController field and two method calls. Deploy and verify the arm still reaches the setpoint with the same kP, kI, kD gains — behavior should be nearly identical or slightly improved due to derivative-on-measurement.
Add setTolerance() to the constructor with a position tolerance of 2° and velocity tolerance of 10°/s. Implement isAtTarget() wrapping m_pid.atSetpoint(). In your arm command's isFinished(), use isAtTarget(). Command the arm to 90°. Observe on SmartDashboard: the Arm/AtTarget boolean should become true only when the arm has settled — not during the swing. Time how long it takes to settle.
Add setIntegratorRange(−1.5, 1.5) and setIZone(5.0). Test windup: hold the arm against a hard stop for 10 seconds while the command targets 90°. Release. Confirm the arm moves to 90° without a large overshoot spike from the I term. Without these protections, the arm would overshoot dramatically after being held away from target.
If you have an angular mechanism (swerve steering or turret), add enableContinuousInput(−180, 180) (or appropriate range). Test by commanding across the wrap boundary. Confirm the module takes the short path, not the long one. Log both the commanded angle and the actual path length from SmartDashboard to verify.
Stretch goal: Use NetworkTables / SmartDashboard to tune gains at runtime. In the subsystem's periodic(), read kP, kI, kD from SmartDashboard: double kP = SmartDashboard.getNumber("Arm/kP", Constants.Arm.kP). Call m_pid.setPID(kP, kI, kD). This lets you change gains without redeploying code — dramatically faster for tuning sessions. Add a safety: clamp each gain to a reasonable range before passing to setPID so an accidental large value doesn't damage hardware.