PID Control in Subsystems

In the previous lessons, we learned how to turn a motor on and off. But what if you have a robot arm, an elevator, or a turret that needs to move to an exact angle or height and hold that position perfectly?

If you just tell the motor to turn on and then try to turn it off when it reaches the target, the momentum of the heavy arm will carry it past the target. If you try to reverse it, it will overshoot the other way. This leads to the robot violently shaking back and forth!

To solve this, our team uses a PID Controller.

What is a PID Controller?

A PID Controller is a mathematical algorithm that calculates exactly how much power to give a motor based on how far away it is from its target.

• P (Proportional): The further away the arm is from the target, the harder the motor pushes. As it gets closer, it slows down gently to avoid overshooting.
• I (Integral): If the arm is stuck just below the target (maybe it's too heavy for the P term to push it the rest of the way), the I term slowly builds up extra power over time to nudge it exactly to the target.
• D (Derivative): This acts like a brake. If the arm is moving too fast towards the target, the D term slows it down to prevent overshooting.

1. Positional PID (e.g., A Turret)

Let's look at a real example from our team's code. This is a TurretSubsystem that rotates a shooter turret to a specific angle using an encoder and a PID controller.

package org.firstinspires.ftc.teamcode.subsystems;
 
import com.arcrobotics.ftclib.command.SubsystemBase;
import com.arcrobotics.ftclib.controller.PIDController;
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
 
public class TurretSubsystem extends SubsystemBase {
    
    private final MotorEx turretMotor;
    private final PIDController pid;
 
    // Tuning coefficients (Proportional, Integral, Derivative)
    public static double kP = 0.008;
    public static double kI = 0.0;
    public static double kD = 0.0001;
 
    public TurretSubsystem(HardwareMap hardwareMap) {
        this.turretMotor = new MotorEx(hardwareMap, "turretMotor");
        this.turretMotor.setRunMode(MotorEx.RunMode.RawPower); 
        
        // Initialize the PID Controller with our tuning values
        this.pid = new PIDController(kP, kI, kD);
    }
 
    /**
     * Calculates the PID output and spins the turret toward the target angle.
     * This method would be called repeatedly by a Command's execute() method!
     */
    public void alignToAngle(double currentAngle, double targetAngle) {
        // Calculate required motor power based on the error (target - current)
        double power = pid.calculate(currentAngle, targetAngle);
 
        // Clip power for safety (e.g., never go faster than 50% speed)
        power = Math.max(-0.5, Math.min(0.5, power));
 
        turretMotor.set(power);
    }
}

How to Tune a Position PID

You cannot just guess the kP, kI, and kD values; you must tune them on the physical robot. We use FTC Dashboard to change these variables live without having to re-compile the code.

Here is the standard tuning process:

1. Set everything to 0: Start with kP = 0, kI = 0, and kD = 0.
2. Tune P: Slowly increase kP and command the turret to move. Keep increasing kP until the turret moves quickly to the target but slightly overshoots and oscillates (shakes back and forth).
3. Tune D: Slowly increase kD. The D term acts as a damper. It should stop the shaking and allow the turret to snap to the target smoothly.
4. Tune I (Optional): If friction causes the turret to get stuck just before reaching the target, add a very tiny amount of kI. The I term will slowly build up power until the turret nudges into the perfect spot.

2. Velocity PIDF (e.g., A Flywheel)

A shooter flywheel is completely different from a turret. We don't want the flywheel to stop at a specific position; we want it to spin continuously at a specific velocity (like 2000 RPM).

For velocity, standard PID isn't enough. We add Feedforward (F). Feedforward uses physics math to guess exactly how much voltage the motor needs to hold that speed, taking a massive load off the PID controller.

Here is a simplified version of the LaunchSubsystem from our team's repository:

package org.firstinspires.ftc.teamcode.subsystems;
 
import com.arcrobotics.ftclib.command.SubsystemBase;
import com.arcrobotics.ftclib.controller.PIDController;
import com.arcrobotics.ftclib.controller.wpilibcontroller.SimpleMotorFeedforward;
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
 
public class LaunchSubsystem extends SubsystemBase {
    
    private final MotorEx launchMotor;
    private final PIDController flywheelPID;
    private SimpleMotorFeedforward flywheelFF;
 
    // PID Coefficients
    public static double kP = 0.005;
    public static double kI = 0.0;
    public static double kD = 0.0001;
 
    // Feedforward Coefficients
    public static double kS = 0.05; // Static friction
    public static double kV = 0.012; // Velocity gain
 
    public LaunchSubsystem(HardwareMap hardwareMap) {
        launchMotor = new MotorEx(hardwareMap, "launchMotor");
        launchMotor.setRunMode(MotorEx.RunMode.RawPower);
 
        flywheelPID = new PIDController(kP, kI, kD);
        flywheelFF = new SimpleMotorFeedforward(kS, kV);
    }
 
    public void updateFlywheel(double targetTPS, double currentTPS) {
        
        // 1. Calculate PID (The Correction)
        // If the flywheel slows down when a ball goes through, this adds extra power.
        double pidOutput = flywheelPID.calculate(currentTPS, targetTPS);
 
        // 2. Calculate Feedforward (The Prediction)
        // This calculates the base power needed just to maintain the target speed in a vacuum.
        double ffOutput = flywheelFF.calculate(targetTPS);
 
        // 3. Combine them!
        double totalPower = pidOutput + ffOutput;
 
        launchMotor.set(totalPower);
    }
}

How to Tune a Velocity PIDF

Tuning a flywheel is the opposite of tuning a turret: you tune the Feedforward first.

1. Set everything to 0: kP = 0, kI = 0, kD = 0, kS = 0, kV = 0.
2. Tune kV: Increase kV until the motor reaches the target speed. It won't be perfectly stable, but it should hover around the target speed based purely on the Feedforward prediction.
3. Tune kS (Optional): If the motor refuses to start spinning at very low target speeds because of internal friction, add a tiny bit of kS (Static Friction) to give it a kickstart.
4. Tune P: Now that Feedforward is doing 90% of the work, add kP. This allows the PID controller to aggressively react and add power if the flywheel drops in speed (like when a heavy game piece passes through it).
5. Tune D (Optional): Add kD if the flywheel overshoots its target RPM while recovering from a shot.

Check Your Understanding