Unit 8 · Lesson 6

P, I, and D Terms Individually

Lesson 5 explained the concepts. This lesson puts code to them. You will write each term from scratch — a single line for P, a three-line loop for D, a three-field system for I — and understand exactly what each piece of Java does. By the end, you'll have a complete manual PID implementation and understand every line of it before handing control over to WPILib's PIDController in Lesson 7.

By the end of this lesson, you will:

Write a P-only controller in one line and explain its sole failure mode
Write a D term using a stored previous-error field and explain why the derivative amplifies noise
Write an I term using a running accumulator field, with a hard clamp to prevent windup
Combine all three into a complete manual PID implementation with correct field initialization
Use the interactive term toggle to observe what each term contributes to the total response curve
State two reasons WPILib's PIDController is better than a manual implementation

The Setup: One Subsystem, Three Fields

We'll build PID into an arm subsystem. The arm has a target angle (setpoint), a current angle (from the encoder), and a motor. All three PID fields live in the subsystem class — not in the command. Commands pass the setpoint; the subsystem owns the controller state.

The three state fields a manual PID controller needs:

double m_prevError — stores the previous loop's error, required by the D term
double m_integralSum — accumulates past errors over time, required by the I term
The setpoint itself — passed as an argument to the calculate method, not stored

Unlike SimpleMotorFeedforward, which is stateless, a PID controller is stateful: it remembers what happened in previous loops. This is why resetting the controller when it becomes inactive matters — a wound-up I term from a previous command can spike the output when the controller resumes.

The P Term: One Line of Code

The proportional term is the simplest: multiply the current error by kP. Every other line in the subsystem around it is bookkeeping — the P term itself is a single multiplication.

ArmSubsystem.java — P-only controller

// Constants
private static final double kP = 0.05; // V per degree of error

// Called from execute() every 20 ms
public void moveToAngle(double setpointDeg) {
  double currentDeg = getAngleDegrees();

  // P term: proportional to current error
  double error = setpointDeg - currentDeg;
  double pOutput = kP * error;

  // Apply — setVoltage() not set()
  m_motor.setVoltage(MathUtil.clamp(pOutput, -12.0, 12.0));

  // Publish for tuning
  SmartDashboard.putNumber("Arm/Error", error);
  SmartDashboard.putNumber("Arm/POutput", pOutput);
}

← click a highlighted token

// State fields — declared in the class, not in execute()
private double m_prevError = 0.0;
private static final double kP = 0.05;
private static final double kD = 0.002; // V·s per degree
private static final double kDt = 0.020;// 20 ms loop period

public void moveToAngle(double setpointDeg) {
  double currentDeg = getAngleDegrees();
  double error = setpointDeg - currentDeg;

  // P term
  double pOutput = kP * error;
  // D term: rate of change of error
  double errorRate = (error - m_prevError) / kDt;
  double dOutput = kD * errorRate;
  m_prevError = error; // must happen every loop

  double output = pOutput + dOutput;
  m_motor.setVoltage(MathUtil.clamp(output, -12.0, 12.0));
  SmartDashboard.putNumber("Arm/DOutput", dOutput);
}

← click a highlighted token

// All three state fields
private double m_prevError = 0.0;
private double m_integralSum = 0.0;
private static final double kP = 0.05;
private static final double kI = 0.002; // V·s/deg
private static final double kD = 0.002; // V·s/deg
private static final double kDt = 0.020;
private static final double kIMax = 2.0; // V max I contribution

public void moveToAngle(double setpointDeg) {
  double currentDeg = getAngleDegrees();
  double error = setpointDeg - currentDeg;

  // P
  double pOutput = kP * error;

  // I — accumulate error × dt, then clamp
  m_integralSum += error * kDt;
  m_integralSum = MathUtil.clamp(m_integralSum,
                            -kIMax/kI, kIMax/kI);
  double iOutput = kI * m_integralSum;

  // D
  double errorRate = (error - m_prevError) / kDt;
  double dOutput = kD * errorRate;
  m_prevError = error;

  double output = pOutput + iOutput + dOutput;
  m_motor.setVoltage(MathUtil.clamp(output, -12.0, 12.0));
}

/** Call when disabling or changing setpoints to prevent windup. */
public void resetPID() {
  m_prevError = 0.0;
  m_integralSum = 0.0;
}

← click a highlighted token

Interactive: Toggle Each Term On and Off

Enable and disable each PID term to see its contribution to the response curve. Start with only P enabled, then add D and I one at a time.

Step response — arm commanded from 0° to 90°

Active terms:

kP: 1.5

Enable terms above to see their effect on the response. Start with P only, then add D, then add I.

The D Term: Why It Needs a Previous Error

The derivative term measures how fast the error is changing — its rate of change. In calculus this is de/dt. In a 20 ms discrete loop it becomes (current error − previous error) / 0.020. To compute this, the controller must remember what the error was last loop. That's why m_prevError is a field, not a local variable. A local variable disappears at the end of each method call. A field persists between calls.

The most important line in the D implementation is the one that gets skipped most often: m_prevError = error; at the end of every loop. If you forget this update, the D term always compares the current error to zero — effectively computing the error itself rather than its change rate, producing a wildly wrong output.

⚠️ Derivative kick: why setpoint changes cause a voltage spike

When the setpoint suddenly changes — from 45° to 90° — the error jumps from a small value to a large one in a single 20 ms cycle. The D term sees (large error − small previous error) / 0.020 and produces an enormous output spike. This is called derivative kick, and it can damage mechanisms or trip breakers. WPILib's PIDController solves this by computing the derivative on the measurement (current angle) rather than the error — measurement doesn't jump when the setpoint changes, only when the mechanism actually moves. This is one of the primary reasons to use PIDController instead of a manual implementation.

The I Term: The Accumulator and Windup

The integral term is a running sum: every loop cycle, add (current error × dt) to the accumulator. This means the I term grows whenever error is nonzero and decays (goes negative) when the mechanism overshoots. After enough loops with a constant small error, the accumulator is large enough that kI × sum exceeds the friction, and the arm moves the last fraction of a degree to the setpoint.

The accumulator never resets on its own. If the mechanism is held against a hard stop for 5 seconds with a 10° error, the sum grows to 10 × 5 / 0.020 × 0.020 = 50 degree-seconds. When the mechanism is released, kI × 50 is a large voltage spike. This is integral windup, and the manual fix is clamping the sum before multiplying by kI.

Integral accumulator — watch the sum build up over time

Error (°):

0.0°

Integral sum (°·s):

0.00

I output (kI=0.1):

0.00 V

Clamp limit (±2V/kI):

±20.0 °·s

Press a simulation button to see how the integral accumulator builds over time. The clamp prevents the sum from exceeding ±20 °·s (which corresponds to ±2V of integral output at kI=0.1). Without the clamp, the windup scenario would produce unbounded output.

From Manual to WPILib: What the Library Adds

The manual implementation in the Full PID tab is educationally complete — you understand every line. But WPILib's PIDController is strictly better in production for five reasons:

Derivative on measurement, not error — eliminates derivative kick when setpoints change suddenly
Built-in continuous input — enableContinuousInput(−π, π) handles wrap-around for angular mechanisms (steering motors, turrets) without any extra code
Integrator range clamping — setIntegratorRange(min, max) prevents windup more cleanly than manual clamping
I-zone — setIZone(degrees) only activates the I term when close to the setpoint, preventing windup during large transients
At-setpoint detection — atSetpoint() uses configurable tolerance to reliably detect when the mechanism has arrived, usable in command isFinished()

Lesson 7 covers the complete PIDController API. For now: you understand what it's doing internally because you've written it yourself.

🔍 LRI Perspective: "I ask programmers to explain each line of their PID"

When I inspect a robot with a PID controller, I ask the programmer a simple question: "Walk me through what your kD term does." The ones who have only used WPILib's PIDController as a black box say "it prevents overshoot." The ones who have written it manually say "it computes the rate of change of error — current minus previous divided by dt — and applies a braking force proportional to how fast we're approaching the target, which reduces overshoot." Both robots may work identically. But when something goes wrong at 7 AM before qualifications, only one programmer can debug it in five minutes. Writing PID from scratch once is not a waste of time. It is the investment that makes everything else faster.

🔌 System Check — Validating Each Term in Isolation

Before combining all three terms, validate each in isolation:

P only: Set kI=0, kD=0. Command the arm to a position 30° away. Confirm it moves toward the setpoint and slows as it approaches. Increase kP until oscillation appears, then back off 20%. This is your working kP.
Add D: Set kD to a small value (start at kP/10). Confirm oscillation reduces or disappears. Monitor SmartDashboard's DOutput — during a fast approach it should be negative (braking). If DOutput oscillates rapidly with no mechanism motion, encoder noise is being amplified — reduce kD or add encoder filtering.
Add I only if needed: If the mechanism reaches within 1–2° of the setpoint and stops (steady-state error), add a small kI (start at kP/100). Watch the integral sum on SmartDashboard — it should grow slowly and then bring the mechanism to the setpoint. If it overshoots after reaching the setpoint, kI is too large.
Publish pOutput, iOutput, dOutput, and total output to SmartDashboard as separate channels. In a well-tuned controller near the setpoint, pOutput should be small, iOutput (if nonzero) should be the dominant term eliminating final error, and dOutput should be near zero at steady state.

Knowledge Check

1. You run a P-only controller on an arm targeting 90°. The arm stops at 87.5° and stays there. You increase kP by 50% — the arm now oscillates between 86° and 94°. What is the correct next step?

A Increase kP further — more proportional gain will overcome the remaining 2.5° error.
B Switch to bang-bang control — PID is not appropriate for this mechanism.
C Reduce kP back to the previous non-oscillating value, then add kD to damp the oscillation. Once stable, add a small kI to eliminate the 2.5° steady-state error. Increasing kP past the oscillation threshold is the wrong direction — that's the boundary between stable and unstable.
D Accept the 2.5° error — steady-state error is inherent to PID and cannot be eliminated.

2. The D term formula is (error - m_prevError) / kDt. You forget to update m_prevError = error at the end of the loop. What happens?

A The D term outputs zero — the previous error defaults to the current error each cycle.
B The D term computes (error - 0) / 0.020 = error / 0.020 every cycle, which is 50× the error. This is equivalent to a kD that's 50× larger than intended, producing severe oscillation. The field is initialized to 0.0 and never updated, so every loop computes the derivative as if the previous error was zero.
C No effect — m_prevError defaults to the current error automatically in Java.
D The program crashes with a NullPointerException on the division.

3. The integral clamp in the Full PID tab is: m_integralSum = MathUtil.clamp(m_integralSum, -kIMax/kI, kIMax/kI). Why is the clamp limit kIMax/kI rather than just kIMax?

A To normalize the sum to be between -1 and +1.
B The goal is to limit the output (kI × sum) to ±kIMax volts. Since output = kI × sum, the sum must stay within ±kIMax/kI to keep the output within ±kIMax. Clamping the sum directly to ±kIMax would only be correct if kI = 1.0, which it never is.
C To prevent the integral from growing faster than the proportional term.
D kIMax/kI converts from degrees to radians for the arm feedforward.

💪 Practice Prompt

Build and Test Each Term Individually

In your arm (or elevator) subsystem, implement the P-only version from the tab above. Use kP = 0.01 as a starting value. Deploy and command the mechanism to a position 30° away. Publish error and pOutput to SmartDashboard. Increase kP in steps of 0.01 until slight oscillation appears. Record the oscillation threshold kP — this is your reference for the tuning methodology in Lesson 9.
Add the D term. Set kD = kP / 20 as a starting value. Confirm that oscillation from step 1 is reduced or eliminated. Publish dOutput separately. During a fast approach, verify dOutput is opposite in sign to pOutput (D is braking, not accelerating). If dOutput oscillates rapidly when the mechanism is still, reduce kD — you're amplifying encoder noise.
Add the I term only if the mechanism stops 1°+ short of the setpoint. Start at kI = kP / 100. Implement the clamp exactly as shown. Set kIMax to 1.0V (kIMax/kI gives your sum clamp limit). Observe the integral sum on SmartDashboard — it should rise slowly and then the mechanism should creep to the setpoint. If overshoot appears, reduce kI by 50%.
Add the resetPID() method. Call it from the command's initialize(). Verify: command the arm to 45°, let it settle, then immediately command it to 90°. Without the reset, a wound-up integral from the first command can cause the second command to overshoot. With the reset, the second command starts clean.
Stretch goal: Add separate SmartDashboard channels for pOutput, iOutput, and dOutput alongside the total output. During a typical move-and-hold cycle, watch the relative magnitudes. Near the setpoint: pOutput should dominate initially, dOutput should spike during fast approach then fade, iOutput should grow slowly during the final settling. This three-way breakdown is the most useful real-time tuning view available without AdvantageScope.