12. PID Feedback#

Why it matters#

"Set the motor to 0.3 and hope" doesn't work. Battery voltage changes during a match, mechanisms wear, game pieces add weight, and carpet adds friction. A fixed motor output gets a different result every time.

Feedback control measures where the mechanism actually is, compares it to where you want it, and corrects automatically, 50 times a second. PID is the most common feedback controller in FRC, and in robotics generally. Once you understand it, you can make arms hold their angle, flywheels hold their speed, and drivetrains stop exactly where you tell them.

Read this, then come back#

• Introduction to Control Theory
• Introduction to PID
• PID Control in WPILib
• Tuning a Flywheel Velocity Controller and Tuning a Vertical Arm Position Controller

Walkthrough#

Open loop vs. closed loop#

• Open loop: "Run the motor at 30%." The code never checks the result.
• Closed loop: "Get the arm to 90°." Measure, compare, correct, repeat every 20 ms.

The loop is closed because the sensor's measurement feeds back into the next decision.

The WPILib docs draw it like this, using the standard control-theory names:

A closed loop. The **plant** is the mechanism being controlled. From the WPILib docs, Control System Basics (CC BY 4.0).

The vocabulary#

Error is everything#

error = setpoint − measurement

• Positive error: you're short of the target, so push forward.
• Negative error: you're past it, so push back.
• Zero error: you're there.

The controller's whole job is to drive the error to zero and keep it there.

Build PID one letter at a time#

P: Proportional. "Push harder the farther away you are."#

output = kP × error

With kP = 0.02 and the arm 30° short, the output is 0.02 × 30 = 0.6. At 5° short, it's 0.1. As the arm approaches the target, the push gets gentler.

A P controller: the output is K_p times the error. From the WPILib docs, Introduction to PID (CC BY 4.0).

P alone has a classic flaw: steady-state error. Near the target, the error is small, so the output is small, and a small output may not beat friction or gravity. The mechanism stops a little short and stays there.

D: Derivative. "Slow down as you approach."#

output += kD × (how fast the error is changing)

The derivative is the rate of change of the error. When the mechanism is rushing toward the target, the error is shrinking fast, and D pushes against that motion. It acts like a shock absorber.

A PD controller adds a second term based on how fast the error is changing. From the WPILib docs, Introduction to PID (CC BY 4.0).

• With D, you can use a higher kP for a faster response without as much overshoot.
• Too much D makes the mechanism sluggish, and because it reacts to change, it amplifies sensor noise into jittery output.

I: Integral. "Fix the leftover error."#

output += kI × (error added up over time)

The integral keeps a running total of the error. If the mechanism sits a little short of the target for a while, that total grows until the output is big enough to finish the job. That's how I removes steady-state error.

Steady-state error: P control alone leaves a gap below the setpoint. From the WPILib docs, Introduction to PID (CC BY 4.0).

Left: adding I removes the gap. Right: too much I causes overshoot. From the WPILib docs, Introduction to PID (CC BY 4.0).

All three together#

output = kP × error  +  kI × ∫error  +  kD × d(error)/dt
         (present)       (past)          (future trend)

Watching the response#

When you tune, plot the setpoint and measurement together on a graph.

The three response types. From the WPILib docs, Introduction to PID (CC BY 4.0).

These shapes tell you what to change:

The PIDController class#

WPILib does the math for you:

import edu.wpi.first.math.controller.PIDController;

PIDController pid = new PIDController(kP, kI, kD);

// Every loop:
double output = pid.calculate(measurement, setpoint);

Useful extras:

Code it: an arm that goes to an angle#

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.telemetry.Telemetry;
import frc.robot.telemetry.TelemetryTable;

public class Arm extends SubsystemBase {
    private final PWMSparkMax motor = new PWMSparkMax(1);
    private final Encoder encoder = new Encoder(0, 1);
    private final PIDController pid = new PIDController(0.05, 0.0, 0.002);
    private final TelemetryTable telemetry = Telemetry.getTable("Arm");   // Lesson 6.1

    public Arm() {
        encoder.setDistancePerPulse(ArmConstants.DEGREES_PER_PULSE);
        pid.setTolerance(2.0);   // within 2° counts as "there"
    }

    public double getAngleDegrees() {
        return encoder.getDistance();
    }

    /** Moves to the angle and ends once it's there. */
    public Command goTo(double degrees) {
        return runOnce(pid::reset)
            .andThen(run(() -> {
                double output = pid.calculate(getAngleDegrees(), degrees);
                motor.set(MathUtil.clamp(output, -0.5, 0.5));   // limit power while tuning
            }))
            .until(pid::atSetpoint)
            .withName("Arm to " + degrees);
    }

    /** Moves to the angle and keeps holding it until interrupted. */
    public Command hold(double degrees) {
        return runOnce(pid::reset)
            .andThen(run(() -> motor.set(pid.calculate(getAngleDegrees(), degrees))))
            .withName("Hold " + degrees);
    }

    @Override
    public void periodic() {
        telemetry.log("Angle", getAngleDegrees());
        telemetry.log("Setpoint", pid.getSetpoint());
        telemetry.log("Error", pid.getError());
        telemetry.log("Output", motor.get());
    }
}

Units of the gains#

The gains depend on your units. With degrees in and motor percent out, kP = 0.02 means "2% output per degree of error." Measure in radians instead and the same behavior needs kP ≈ 1.15. If you change units, you must retune. That's one more reason to pick units once and keep them (Lesson 8).

A tuning recipe#

1. Check the sensor first. Move the mechanism by hand. Does the measurement go up when the motor would push it positive? If the sensor and motor disagree on direction, PID will push the wrong way and run away.
2. Set kI = 0 and kD = 0.
3. Start with a small kP. Raise it (doubling works well) until the mechanism oscillates around the target.
4. Back kP off by about half.
5. Raise kD in small steps until the overshoot is gone, or it starts to jitter.
6. If it still settles short, add feedforward (Lesson 13) before reaching for kI.
7. Write down the gains that work, and put them in Constants.java.

Tuning live from the dashboard#

Rebuilding and redeploying for every gain change is slow. Make the gains tunable with Tunables (Lesson 6.1), and copy them into the controller every loop:

private final TunableDouble kP = Tunables.addDouble("Arm/kP", 0.05);
private final TunableDouble kI = Tunables.addDouble("Arm/kI", 0.0);
private final TunableDouble kD = Tunables.addDouble("Arm/kD", 0.002);

@Override
public void periodic() {
    pid.setPID(kP.get(), kI.get(), kD.get());
    // ...the telemetry.log lines from above...
}

Change kP, kI, and kD in Elastic (as Text Display widgets) and watch the response immediately. When you're happy, copy the numbers into Constants.java. Dashboard edits are lost when the robot restarts.

On-controller PID#

Modern motor controllers (TalonFX, SPARK MAX, SPARK Flex) can run PID on the controller itself, at 1000 Hz instead of 50 Hz, using the motor's built-in encoder. Many teams use this for flywheels and arms. The ideas and tuning process are the same; only where the math runs is different. Learn it with WPILib's PIDController first, then read your vendor's docs.

Try it#

Tune a turret, right here#

This is WPILib's turret simulator, running PID only. Click a spot around the turret to set a new angle, or click and drag the pointer to make the setpoint move smoothly.

Interactive simulator from the WPILib docs, Tuning a Turret Position Controller (CC BY 4.0).

• Set kP, kI, kD, and kV to zero. Raise kP until the turret moves sharply to each new angle. If it oscillates too much, lower kP until it stops.
• Raise kD to reduce the "lag" when you drag the setpoint smoothly. If it starts to oscillate, lower kD.
• Turn on System Noise. Does your tuning still work?
• Try the arm tuning simulator with PID only. Notice how hard it is to hold the arm level, and how it sags at some angles. Write down your best kP and kD. You'll compare in the next lesson.
• In the ArmSimulation example project, replace its controller with your goTo and hold commands, bind them to two buttons, and tune them with the dashboard
• Graph Arm/Angle and Arm/Setpoint together in one Elastic Graph widget while you tune

Check yourself#