What is a PID Controller and Why Do You Need One?

In robocup soccer, your robot needs to do things like:

• Drive straight while maintaining a specific heading
• Control motor speeds to reach target velocities
• Follow a ball at a certain distance
• Rotate to face a specific angle

All of these are control problems. You have a target value (setpoint) and a current value (sensor reading), and you need to figure out what output to send to your motors to minimize the error between them.

A PID controller solves this problem. It takes the error and computes an output using three components:

P (Proportional): How far off are we?

• Error = 10° → Big correction
• Error = 1° → Small correction

I (Integral): Have we been off-target for a while?

• If we’ve been consistently 2° off for 5 seconds, gradually increase correction
• Eliminates steady-state error caused by friction, battery voltage drop, etc.

D (Derivative): How fast is the error changing?

• Error was 10° and now it’s 5° → We’re approaching target, ease off
• Prevents overshooting and oscillations

Note: This implementation includes several important fixes to common PID bugs including proper initialization, correct integral and derivative calculations, and angle wrapping for heading control.

The Code Structure

Here’s the class interface:

class PIDController {
  public:
    PIDController(const double setpoint, const double min, const double max,
                  const double kp, const double ki, const double kd,
                  const uint32_t minDt = 0, const double maxi = infinity(),
                  const double maxSetpointChange = infinity());

    // Main update function - call this every loop
    double advance(const double input, const double scaler = 1.0,
                   const bool isAngle = false);

    // Reset the controller (clears integral, sets everything to zero)
    void reset();

    // Update parameters on the fly
    void updateSetpoint(const double value);
    void updateLimits(const double min, const double max);
    void updateGains(const double kp, const double ki, const double kd);

    // Debug output to serial
    void debugPrint(const char *name = nullptr, Stream &serial = Serial);

    double currentSetpoint() const { return _setpoint; }
};

Creating a PID Controller

Basic Example: Heading Control

// Keep the robot facing 0° (forward)
// Output range: -255 to 255 (motor PWM values)
// Gains: kp=2.0, ki=0.01, kd=0.5
PIDController headingPID(0.0, -255.0, 255.0, 2.0, 0.01, 0.5);

void loop() {
    double currentHeading = imu.getYaw(); // Read IMU
    double correction = headingPID.advance(currentHeading);

    // Apply correction to motors
    leftMotor.setPWM(baseSpeed - correction);
    rightMotor.setPWM(baseSpeed + correction);
}

Constructor Parameters Explained

PIDController(setpoint, min, max, kp, ki, kd, minDt, maxi, maxSetpointChange)

• setpoint: Target value you want to reach
• min, max: Output limits (e.g., -255 to 255 for motor PWM)
• kp, ki, kd: PID gain constants (more on tuning later)
• minDt: Minimum time (microseconds) between updates. Use 0 to update every loop
• maxi: Maximum value for integral accumulator. Default infinity() means no limit
• maxSetpointChange: Maximum rate of setpoint change per update. Default infinity() means instant changes

How the Controller Works Internally

The advance() Function

This is the heart of the controller. Call it every loop with your current sensor reading:

double advance(const double input, const double scaler = 1.0, const bool isAngle = false)

Parameters:

• input: Current measured value from your sensor
• scaler: Velocity scaling factor (explained in next section)
• isAngle: Set to true if controlling angles (currently unused, kept for future angle wrapping)

Returns: The control output to apply to your actuators

What Happens Inside advance()

// 1. Handle first iteration
if (_justStarted) {
    _justStarted = false;
    return 0;  // Don't do anything on first call
}

// 2. Ignore invalid sensor readings
if (std::isnan(input)) return 0;

// 3. Gradually move setpoint toward target (if maxSetpointChange is set)
const auto dsetpoint = constrain(_targetSetpoint - _setpoint,
                                 -_maxSetpointChange, _maxSetpointChange);
_setpoint += dsetpoint;

// 4. Check if enough time has passed (if minDt is set)
if (micros() - _lastTime < _minDt) return _lastOutput;

// 5. Calculate time delta
const auto now = micros();
const auto dt = now - _lastTime;
_lastTime = now;

// 6. Calculate error
auto error = _setpoint - input;

// 7. Normalize angle errors to [-180, 180] (if isAngle is true)
if (isAngle) {
    while (error > 180.0) error -= 360.0;
    while (error < -180.0) error += 360.0;
}

// 8. Update integral (accumulated error over time)
_integral += error * dt;
_integral = constrain(_integral, -_maxi, _maxi);  // Anti-windup

// 9. Calculate P, I, D components (FIXED FORMULAS)
const auto p = (_kp * scaler) * error;
const auto i = (_ki * _kp * scaler) * _integral;  // NO division by dt!
const auto d = (_kd * _kp * powf(scaler, 2)) * (error - _lastError) / dt;  // Proper derivative

// 10. Combine and limit output
const auto output = constrain(p + i + d, _min, _max);

// 11. Store values for next iteration
_lastOutput = output;
_lastError = error;

return output;

Important Implementation Details

Integral Term Fix: The integral is calculated as ki * kp * _integral, NOT ki * kp / dt * _integral. This is crucial because _integral already accumulates error * dt each iteration:

_integral += error * dt;  // Accumulates error over time
const auto i = (_ki * _kp * scaler) * _integral;  // Just multiply, don't divide by dt

Dividing by dt here would be wrong because you’re dividing by the current dt, but _integral contains error * dt from many previous iterations with potentially different dt values. This would make the I term behave inconsistently as loop timing varies.

Derivative Term Fix: The derivative is calculated as (error - _lastError) / dt, which is the correct rate of change. Previous versions had an extra dt * in the numerator that canceled with the denominator, making the D term not actually compute a rate of change:

// WRONG: dt cancels out, this is just proportional to error difference
const auto d = (_kd * _kp * dt * ...) * (error - _lastError) / dt;

// CORRECT: actual rate of change
const auto d = (_kd * _kp * powf(scaler, 2)) * (error - _lastError) / dt;

Why Integral Windup is a Problem

Imagine your robot is held against a wall, unable to move. The heading controller wants to turn 45°:

Error = 45° - 0° = 45°

Every loop iteration, this 45° error gets added to _integral:

_integral += error * dt;  // Keep accumulating...

After 1 second: _integral might be 45000 (45° × 1000ms) After 2 seconds: _integral might be 90000

When you finally release the robot, the I term is HUGE:

i = ki * _integral  // Massive value!

The robot violently overshoots because all that accumulated error demands maximum correction. The I term takes forever to “wind down” even after the robot passes the target.

The Fix: Anti-Windup

_integral = constrain(_integral, -_maxi, _maxi);

This limits how much error can accumulate. When setting maxi in the constructor, think about the maximum I contribution you want:

// If you want max I contribution of 50 PWM units and ki = 0.01:
double maxi = 50 / 0.01;  // = 5000
PIDController pid(0, -255, 255, 2.0, 0.01, 0.5, 0, 5000);

Or just use the default infinity() if you’re not experiencing windup issues.

Angle Wrapping for Heading Control

When controlling heading angles (0-360° or -180 to 180°), you need special handling because angles wrap around. This implementation includes proper angle wrapping when you set isAngle = true.

The Problem

Imagine your robot is facing 350° and you want it to face 10°:

// Without angle wrapping
error = 10 - 350 = -340°

The robot would try to turn 340° backwards (counterclockwise) instead of just 20° forward (clockwise)! This is obviously wrong - you want the shortest path.

The Solution

When isAngle = true, the error is normalized to the range [-180, 180]:

if (isAngle) {
    while (error > 180.0) error -= 360.0;
    while (error < -180.0) error += 360.0;
}

Examples:

Heading 350°, setpoint 10°:

Raw error = 10 - 350 = -340°
Normalized = -340 + 360 = 20° ✓
Robot turns 20° forward (shortest path)

Heading 10°, setpoint 350°:

Raw error = 350 - 10 = 340°
Normalized = 340 - 360 = -20° ✓
Robot turns 20° backward (shortest path)

Heading 359°, setpoint 1°:

Raw error = 1 - 359 = -358°
Normalized = -358 + 360 = 2° ✓
Robot turns 2° forward (tiny adjustment)

Usage for Heading Control

// Create heading controller
PIDController headingPID(0.0, -255.0, 255.0, 2.0, 0.01, 0.5);

void loop() {
    double currentHeading = imu.getYaw();  // 0-360° or -180 to 180°

    // Set isAngle = true for angle wrapping
    double correction = headingPID.advance(currentHeading, scaler, true);

    leftMotor.setPWM(baseSpeed - correction);
    rightMotor.setPWM(baseSpeed + correction);
}

Important: Only use isAngle = true for angular values (heading, orientation). For linear values (speed, distance, position), leave it false (the default).

Velocity Scaling: The Key Feature

This is where this PID implementation gets interesting. Most PID controllers use fixed gains, but robot behavior changes dramatically with speed.

The Problem

When your robot is stationary (0 cm/s):

• Small heading errors are easy to correct
• Motors respond predictably
• You can use aggressive gains

When your robot is moving fast (300 cm/s):

• The same heading error has bigger consequences
• Small steering corrections create large trajectory changes
• You need gentler corrections to avoid oscillations

Using the same PID gains for both situations leads to either:

• Sluggish response at low speeds
• Violent oscillations at high speeds

The Solution: Velocity Scaling

double advance(const double input, const double scaler = 1.0)

The scaler parameter automatically adjusts PID gains based on robot velocity:

const auto p = (_kp * scaler) * error;
const auto i = (_ki * _kp * scaler) * _integral;
const auto d = (_kd * _kp * powf(scaler, 2)) * (error - _lastError) / dt;

Notice how:

• P term scales linearly with scaler
• I term scales linearly with scaler
• D term scales quadratically with scaler²

Practical Example: Heading Control While Driving

void Movement::drive() {
    double velocity = getVelocity();  // cm/s
    double actualHeading = imu.getYaw();

    // Tune PID at reference velocity of 300 cm/s
    // At other speeds, scale proportionally
    double scaler;
    if (velocity == 0) {
        scaler = 0.1;  // Stationary - use very gentle corrections
    } else {
        scaler = velocity / 300.0;  // Scale linearly with speed
    }

    double angularCorrection = headingPID.advance(actualHeading, scaler);

    // Apply to motors
    leftMotor.setPWM(velocity - angularCorrection);
    rightMotor.setPWM(velocity + angularCorrection);
}

What This Does:

At 150 cm/s (half reference speed):

• scaler = 150/300 = 0.5
• P gain effectively becomes kp * 0.5
• I gain effectively becomes ki * 0.5
• D gain effectively becomes kd * 0.25

At 600 cm/s (double reference speed):

• scaler = 600/300 = 2.0
• P gain effectively becomes kp * 2.0
• I gain effectively becomes ki * 2.0
• D gain effectively becomes kd * 4.0

The quadratic D scaling provides extra damping at high speeds, preventing oscillations.

Tuning Your PID Controller

Start with P Only

Set ki = 0 and kd = 0. Increase kp until the system responds to errors:

PIDController pid(0, -255, 255, 1.0, 0, 0);  // P only

• Too low: Sluggish, doesn’t reach setpoint
• Too high: Overshoots and oscillates
• Just right: Reaches setpoint with slight overshoot

Add D to Reduce Overshoot

Keep ki = 0. Increase kd to dampen oscillations:

PIDController pid(0, -255, 255, 1.0, 0, 0.3);  // P + D

• D term opposes rapid changes in error
• Prevents overshoot
• Too much D makes response sluggish

Add I to Eliminate Steady-State Error

If the system settles close to but not exactly at the setpoint, add ki:

PIDController pid(0, -255, 255, 1.0, 0.01, 0.3);  // Full PID

• I term accumulates small persistent errors
• Slowly increases output until error is eliminated
• Start with very small values (0.001 to 0.1)

Tuning Example: Heading Control

// Step 1: P only - tune at reference velocity (300 cm/s)
PIDController heading(0, -255, 255, 2.0, 0, 0);

// Step 2: Add D to reduce oscillations
PIDController heading(0, -255, 255, 2.0, 0, 0.5);

// Step 3: Add I if robot drifts slowly
PIDController heading(0, -255, 255, 2.0, 0.01, 0.5);

// Step 4: Set integral limits if windup occurs
PIDController heading(0, -255, 255, 2.0, 0.01, 0.5, 0, 5000);

Advanced Features

Setpoint Ramping

If you set maxSetpointChange, the controller won’t jump instantly to new setpoints:

// Limit setpoint changes to 1.0 unit per update
PIDController pid(0, -255, 255, 2.0, 0.01, 0.5, 0, infinity(), 1.0);

pid.updateSetpoint(90.0);  // Target 90°
// Internal setpoint ramps: 0 → 1 → 2 → 3 → ... → 90

Why? Prevents sudden jerks when changing targets. Useful for smooth motion planning.

Minimum Update Time

If you set minDt, the controller won’t update faster than that interval:

// Update at most every 10ms (10000 microseconds)
PIDController pid(0, -255, 255, 2.0, 0.01, 0.5, 10000);

Why?

• Derivative term needs reasonable time between samples
• If your main loop runs at 1000 Hz but sensor updates at 100 Hz, prevent calculating D on stale data
• Reduces computational load on slower microcontrollers

Dynamic Parameter Updates

Change gains, limits, or setpoint while running:

PIDController pid(0, -255, 255, 2.0, 0.01, 0.5);

// Change target heading
pid.updateSetpoint(45.0);

// Switch to gentler gains for precision control
pid.updateGains(1.0, 0.005, 0.3);

// Reduce output power
pid.updateLimits(-100, 100);

Debugging with Serial Output

void loop() {
    double output = pid.advance(sensorValue);

    // Print detailed PID state every 100ms
    static uint32_t lastPrint = 0;
    if (millis() - lastPrint > 100) {
        pid.debugPrint("Heading");
        lastPrint = millis();
    }
}

Output:

[Heading] Setpoint: 90.00 | Input: 87.34 | Error: 2.66 | Output: 23.45 | P: 20.00 | I: 2.00 | D: 1.45 | dt: 10

This shows:

• Current setpoint and input
• Error magnitude
• Total output
• Individual P, I, D contributions
• Time delta since last update

Common Pitfalls and Solutions

Pitfall 1: Forgetting to Call reset()

When switching between tasks (e.g., from offense to defense), the accumulated integral from the previous task persists:

// WRONG
void switchToDefense() {
    headingPID.updateSetpoint(180.0);  // Face backwards
    // Old integral from facing forward still active!
}

// CORRECT
void switchToDefense() {
    headingPID.reset();  // Clear integral and error history
    headingPID.updateSetpoint(180.0);
}

Pitfall 2: Inconsistent Loop Timing

PID calculations depend on dt. If your main loop has variable timing, the controller behaves unpredictably:

// BAD: Variable loop time
void loop() {
    doSomeRandomStuff();  // Takes 5-50ms randomly
    double output = pid.advance(sensor.read());
}

// GOOD: Fixed loop time
void loop() {
    static uint32_t lastLoop = 0;
    if (micros() - lastLoop < 10000) return;  // 10ms loop
    lastLoop = micros();

    double output = pid.advance(sensor.read());
}

Pitfall 3: Wrong Output Range

// WRONG: Output doesn't match motor range
PIDController pid(0, -100, 100, 2.0, 0.01, 0.5);  // -100 to 100
analogWrite(motorPin, pid.advance(sensor));  // analogWrite wants 0-255!

// CORRECT: Match PID output to motor input
PIDController pid(0, -255, 255, 2.0, 0.01, 0.5);
analogWrite(motorPin, constrain(127 + pid.advance(sensor), 0, 255));

Pitfall 4: Ignoring Velocity Scaling at Standstill

// PROBLEMATIC
double scaler = velocity / 300.0;  // scaler = 0 when stopped!
// All PID terms become zero, robot can't start moving

// BETTER
double scaler = velocity == 0 ? 0.1 : velocity / 300.0;
// Or: double scaler = max(0.1, velocity / 300.0);

The Math Behind Discrete PID

For those interested, here’s why the code looks the way it does:

Continuous PID Equation:

$$u(t) = K_p e(t) + K_i \int e(t)\,dt + K_d \frac{de(t)}{dt}$$

Discrete Approximation:

Integral:

$$\int e(t)dt \approx \sum e[n] \cdot \Delta t$$

_integral += error * dt;  // Accumulate error × time

Derivative:

$$\frac{de(t)}{dt} \approx \frac{e[n] - e[n-1]}{\Delta t}$$

(error - _lastError) / dt  // Change in error / time

Why the scaling factors:

In the original formulation, all three gains have the same units. This implementation uses a different convention where ki and kd are scaled by kp:

p = kp * error
i = ki * kp * integral     // Note: kp multiplier
d = kd * kp * derivative   // Note: kp multiplier

This lets you think of kp as the “master gain” and ki, kd as relative weights.

Common Implementation Bugs and How They’re Fixed

This implementation includes fixes for several common PID controller bugs that can cause instability or incorrect behavior. Understanding these issues helps you debug problems in other PID code you might encounter.

Bug 1: Uninitialized Setpoint

The Problem:

// WRONG - _setpoint is never initialized
PIDController::PIDController(...)
    : _targetSetpoint(targetSetpoint), _min(min), ...  // Missing _setpoint!

On the first call to advance(), the code does:

_setpoint += dsetpoint;  // Adding to garbage memory!

This causes unpredictable behavior - the robot might jerk violently in random directions on startup.

The Fix:

// CORRECT - initialize both
: _targetSetpoint(targetSetpoint), _setpoint(targetSetpoint), _min(min), ...

Bug 2: Incorrect Integral Calculation

The Problem:

// WRONG - dividing by current dt
_integral += error * dt;
const auto i = (_ki * _kp / dt * scaler) * _integral;

The integral already has dt baked in from accumulation. Dividing by the current dt makes no sense because:

• _integral contains error * dt from many iterations with different dt values
• Dividing by current dt makes the I term weaker when your loop slows down (opposite of what you want)
• The I term essentially becomes another P term instead of actually integrating

The Fix:

// CORRECT - no division by dt
_integral += error * dt;
const auto i = (_ki * _kp * scaler) * _integral;

Bug 3: Incorrect Derivative Calculation

The Problem:

// WRONG - dt cancels out mathematically
const auto d = (_kd * _kp * dt * scaler) * (error - _lastError) / dt;
// This simplifies to: d = (_kd * _kp * scaler) * (error - _lastError)
// Which is NOT a rate of change!

The extra dt * in the numerator cancels with the / dt, so you’re not actually computing a derivative (rate of change), just a difference.

The Fix:

// CORRECT - proper rate of change
const auto d = (_kd * _kp * powf(scaler, 2)) * (error - _lastError) / dt;

Bug 4: Resetting Last Error in Deadband

The Problem:

// WRONG - resets _lastError
if (abs(error) < 2.0) {
    _integral = 0;
    _lastError = 0;  // BAD!
    return 0.0;
}

When error crosses from 1.9° to 2.1°, the D term calculates:

d = (2.1 - 0) / dt  // Huge spike because _lastError was reset to 0!

This causes violent corrections every time you cross the deadband threshold.

The Fix:

// CORRECT - don't reset _lastError
if (abs(error) < 2.0) {
    _integral = 0;
    _lastOutput = 0;
    return 0.0;
    // _lastError keeps its value for smooth D term transition
}

Bug 5: No Angle Wrapping

The Problem:

// For heading control without angle wrapping
error = 10 - 350 = -340°  // Robot tries to turn 340° backwards!

The Fix:

// Normalize to [-180, 180]
if (isAngle) {
    while (error > 180.0) error -= 360.0;
    while (error < -180.0) error += 360.0;
}
// Now error = 20° (shortest path forward)

Testing Your Implementation

If you suspect bugs in your PID code, check for these symptoms:

Symptom: Robot jerks randomly on startup

• Likely cause: Uninitialized setpoint (Bug #1)

Symptom: I term doesn’t eliminate steady-state error, or behaves strangely with varying loop rates

• Likely cause: Incorrect integral calculation (Bug #2)

Symptom: Oscillations don’t dampen, or D term seems ineffective

• Likely cause: Incorrect derivative calculation (Bug #3)

Symptom: Violent corrections when crossing small error thresholds

• Likely cause: Resetting last error in deadband (Bug #4)

Symptom: Robot takes long path when turning near 0°/360°

• Likely cause: No angle wrapping (Bug #5)