Mastering PID Control for FTC Drivetrains

If you’ve ever watched a robot overshoot its target or oscillate back and forth trying to reach a position, you’ve seen what happens without proper control. PID control is the solution—and mastering it will take your autonomous routines from amateur to elite.

What is PID?

PID stands for Proportional-Integral-Derivative. It’s a control loop mechanism that continuously calculates an error value and applies a correction based on three terms:

• P (Proportional): Reacts to the current error
• I (Integral): Accounts for past errors (accumulated over time)
• D (Derivative): Predicts future errors (rate of change)

The Math Behind PID

The output of a PID controller is:

output = Kp * error + Ki * integral + Kd * derivative

Where:

• error = target - current position
• integral = sum of all past errors
• derivative = current error - previous error

Implementing PID in FTC

Here’s a reusable PID controller class:

public class PIDController {
    private double Kp, Ki, Kd;
    private double integral = 0;
    private double previousError = 0;
    private ElapsedTime timer = new ElapsedTime();
    
    public PIDController(double Kp, double Ki, double Kd) {
        this.Kp = Kp;
        this.Ki = Ki;
        this.Kd = Kd;
    }
    
    public double calculate(double target, double current) {
        double dt = timer.seconds();
        timer.reset();
        
        // Prevent huge dt on first iteration
        if (dt > 0.5) dt = 0.02;
        
        double error = target - current;
        
        // Proportional
        double p = Kp * error;
        
        // Integral (with anti-windup)
        integral += error * dt;
        integral = Math.max(-1, Math.min(1, integral)); // Clamp
        double i = Ki * integral;
        
        // Derivative
        double derivative = (error - previousError) / dt;
        double d = Kd * derivative;
        previousError = error;
        
        return p + i + d;
    }
    
    public void reset() {
        integral = 0;
        previousError = 0;
        timer.reset();
    }
}

Using PID for Driving a Distance

public void driveDistance(double inches, double timeout) {
    PIDController pid = new PIDController(0.05, 0.001, 0.02);
    ElapsedTime timer = new ElapsedTime();
    
    double ticksPerInch = 537.7 / (4 * Math.PI); // Adjust for your motors
    int targetTicks = (int)(inches * ticksPerInch);
    
    resetEncoders();
    
    while (opModeIsActive() && timer.seconds() < timeout) {
        int currentPosition = leftMotor.getCurrentPosition();
        double power = pid.calculate(targetTicks, currentPosition);
        
        // Clamp power
        power = Math.max(-0.8, Math.min(0.8, power));
        
        setMotorPowers(power, power);
        
        // Check if we're close enough
        if (Math.abs(targetTicks - currentPosition) < 20) {
            break;
        }
        
        telemetry.addData("Target", targetTicks);
        telemetry.addData("Current", currentPosition);
        telemetry.addData("Power", power);
        telemetry.update();
    }
    
    setMotorPowers(0, 0);
}

Tuning Your PID

Tuning is where the magic happens. Here’s the Ziegler-Nichols method adapted for FTC:

Step 1: Start with P only

Set Ki and Kd to 0. Increase Kp until the robot oscillates consistently. Note this value as Ku (ultimate gain).

Step 2: Measure oscillation period

Time how long one complete oscillation takes. This is Tu.

Step 3: Calculate starting values

Kp = 0.6 * Ku
Ki = 2 * Kp / Tu
Kd = Kp * Tu / 8

Step 4: Fine-tune

From here, adjust:

• Too much overshoot? Reduce Kp or increase Kd
• Too slow? Increase Kp
• Steady-state error? Increase Ki (carefully!)
• Oscillating? Reduce Ki and increase Kd

Common PID Pitfalls

1. Integral windup - Always clamp your integral term
2. Derivative noise - Consider filtering or reducing Kd for noisy sensors
3. Ignoring units - Make sure your gains match your units (degrees vs radians, ticks vs inches)
4. Not resetting - Reset your PID controller between moves

Beyond Basic PID

Once you’ve mastered basic PID, explore:

• Feedforward control - Add a baseline power for known movements
• Motion profiling - Smooth acceleration and deceleration curves
• Cascaded PID - Inner and outer control loops
• Road Runner - A library that handles much of this for you

Recommended Tuning Values to Start

Application	Kp	Ki	Kd
Drive distance	0.03-0.08	0.001-0.01	0.01-0.05
Turn to heading	0.02-0.05	0.001-0.005	0.005-0.02
Arm position	0.005-0.02	0-0.001	0.001-0.01

Note: These are starting points. Your robot will need specific tuning.

---

PID control is a skill that takes practice. Don’t get discouraged if your first attempts aren’t perfect—even experienced teams spend hours tuning. The payoff is worth it!