Lab 9: Turn the Wheel Exactly 10 Times with PID#

Why it matters#

In Lab 6 you tried to turn the wheel exactly 10 times. Timing missed by a different amount every run. Bang-bang was consistent, but always overshot, because the wheel was still moving fast when it reached 10.

What you needed was to slow down as you get close, and to correct if you miss. That's feedback control, and PID is how almost every robot does it. In this lab you'll add PID to the same WheelSubsystem one letter at a time, and watch each letter fix the previous attempt's problem.

It's the same problem as driving exactly 2 meters in auto, raising an elevator to a scoring height, or spinning a turret to a target. Get this right on the test bed and you're ready for the elevator in Lab 10.

Before you start: finish Lab 6 and keep its scorecard. Read Lesson 12 (PID) first.

Read this#

• Introduction to PID
• PID Control in WPILib

Walkthrough#

The scorecard#

Add three rows to your Lab 6 scorecard:

Step 1: Add the PID gains#

    // Add these inside kWheelConstants
    public static final double P = 1.0;              // volts per rotation of error
    public static final double I = 0.0;              // volts per (rotation × second) of error
    public static final double D = 0.0;              // volts per (rotation per second)
    public static final double IZone = 0.5;          // only build up I within 0.5 rotations
    public static final double ToleranceRot = 0.02;  // 0.02 rotations ≈ 7°
    public static final double ToleranceRps = 0.1;   // "nearly stopped"

Step 2: Add a PID controller to the subsystem#

Open WheelSubsystem.java. Add the imports:

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

Add the controller as a field, next to the encoder:

    private final PIDController m_pid =
        new PIDController(kWheelConstants.P, kWheelConstants.I, kWheelConstants.D);

PIDController is WPILib's PID class. You give it the three gains: kP, kI, and kD. It doesn't know anything about motors or encoders. You hand it numbers, and it hands back a number.

And at the end of the constructor:

        m_pid.setTolerance(kWheelConstants.ToleranceRot, kWheelConstants.ToleranceRps);
        m_pid.setIZone(kWheelConstants.IZone);       // limits integral windup (Attempt 5)

Step 3: Make the gains tunable#

You'll try a lot of gains in this lab. Redeploying for every one would take forever, so make them tunable from Elastic (Lab 1.1).

Add these fields to the subsystem, below the others:

    // Gains you can change in Elastic. They start at the values in Constants.
    private final TunableDouble m_kP = Tunables.addDouble("Wheel/kP", kWheelConstants.P);
    private final TunableDouble m_kI = Tunables.addDouble("Wheel/kI", kWheelConstants.I);
    private final TunableDouble m_kD = Tunables.addDouble("Wheel/kD", kWheelConstants.D);
    private final TunableDouble m_iZone = Tunables.addDouble("Wheel/IZone", kWheelConstants.IZone);

Import frc.robot.telemetry.TunableDouble and frc.robot.telemetry.Tunables. Then, at the top of periodic(), copy the dashboard's numbers into the controller every loop:

        // Use whatever gains are on the dashboard right now
        m_pid.setPID(m_kP.get(), m_kI.get(), m_kD.get());
        m_pid.setIZone(m_iZone.get());

setPID replaces all three gains at once. Doing it every loop means a change in Elastic takes effect within 20 ms, even in the middle of a move.

In Elastic, add kP, kI, kD, and IZone from Tunables → Wheel as Text Display widgets, so you can type new values. Add a Graph with Position (rot) and Target (rot) from Telemetry → Wheel together.

Warm up in the simulator#

Before touching the real wheel, try the same idea in WPILib's turret simulator below. A turret is a lot like the test bed wheel: a motor turning something to an exact angle, with no gravity. It's running PID only.

Click a spot around the circle to set a new angle.

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

• With kP, kI, and kD at 0, click a new angle. Nothing happens. Why?
• Raise kP until the turret moves sharply to each new angle. If it oscillates, lower it.
• Raise kD until the overshoot settles down.

Now do the same thing on the real wheel.

Attempt 3: P only (slow down as you get close)#

The idea: "Push in proportion to how far away you are." Far away: push hard. Almost there: push gently. That's the P in PID.

volts = kP × error

With kP = 1.0, at 10 rotations away that's 10 V (clamped to 6 V). At 1 rotation away, 1 V. At 0.1 away, 0.1 V.

public Command turnPid(double rotations) {
    return runOnce(() -> {
            setTargetFromHere(rotations);
            m_pid.reset();                       // clear old history
        })
        .andThen(run(() -> {
            double volts = m_pid.calculate(getRotations(), m_target);
            setVolts(MathUtil.clamp(volts, -kWheelConstants.MaxVolts, kWheelConstants.MaxVolts));
        }))
        .until(m_pid::atSetpoint)                // within tolerance AND nearly stopped
        .withTimeout(5.0)                        // give up if it never gets there
        .finallyDo(this::stop)
        .withName("Attempts 3-5: PID");
}

Piece by piece:

First, once:

return runOnce(() -> {
        setTargetFromHere(rotations);
        m_pid.reset();                       // clear old history
    })

Pick the target, 10 rotations from here (like Lab 6). Then m_pid.reset() clears what the controller remembers from the last move: the I term's running total, and the last error the D term compares against. Without it, an old move could kick this one.

The lambda has curly braces { } because it does two things. Each line inside ends with ;.

Then, every loop:

.andThen(run(() -> {
    double volts = m_pid.calculate(getRotations(), m_target);
    setVolts(MathUtil.clamp(volts, -kWheelConstants.MaxVolts, kWheelConstants.MaxVolts));
}))

Until it arrives, or gives up:

.until(m_pid::atSetpoint)
.withTimeout(5.0)
.finallyDo(this::stop)

Bind it on the wheel controller:

    m_wheelController.x().onTrue(m_wheel.turnPid(10));   // Attempts 3 to 5

1. Make sure kI and kD are 0 in Elastic (Tunables → Wheel).
2. Run turnPid(10) three times with kP = 1.0. Record the results.
3. Try kP = 0.3, then 3, then 10. For each, watch Position and Target on your Elastic graph.

What you'll see:

Unlike bang-bang, if the wheel overshoots, P pushes it back. It corrects itself in both directions.

Attempt 4: P + D (stop the overshoot)#

The idea: "Slow down when you're approaching fast." D looks at how fast the error is shrinking and pushes against that motion, like a shock absorber.

No new code: the same turnPid command uses whatever gains are in the controller.

1. Set kP high enough that it overshoots a little.
2. Raise kD in small steps (try 0.02, 0.05, 0.1) until the overshoot disappears.
3. If the output starts to buzz or jitter, kD is too high. Back it off.
4. Run three times with your best gains and record the results.

What you'll see: With good P and D, the wheel moves quickly and slows smoothly without overshooting. It may still stop a hair short of 10, for the same friction reason as Attempt 3.

Attempt 5: P + I + D (fix the leftover error)#

The idea: "If you've been a little short for a while, push a little harder." I adds up the error over time. While the wheel sits just short of 10, that total keeps growing, so the push keeps growing until it's enough to beat friction and finish the move.

Still no new code. Keep your P and D from Attempt 4.

1. Set kI small, like 0.1, and run turnPid(10). Watch Error on the dashboard: after the wheel slows down, the last little bit should creep to 0.
2. Raise kI (try 0.3, then 0.5) until the leftover error goes away quickly.
3. Too much kI makes the wheel overshoot and slowly swing back and forth. Back it off.
4. Run three times with your best gains and record the results.
5. Copy all three gains into Constants.java (dashboard edits are lost on reboot).

What you'll see: With P, I, and D tuned, the wheel moves quickly, slows smoothly, and finishes within the tolerance of 10, run after run, even with the cloth on it.

What you just learned#

Each term fixes the previous attempt's problem: P gets you close, D stops the overshoot, and I cleans up whatever is left.

Commit#

git add .
git commit -m "Lab 9: PID on the wheel"

Try it#

• Complete the scorecard: Attempts 3 to 5, three runs each
• Graph Position and Target for Attempts 2 through 5, and screenshot each
• Hold the cloth against the wheel during Attempts 4 and 5. Which one still reaches 10?
• Press X three times in a row, waiting for each to finish. Does the wheel end at 10, 20, and 30? Compare with bang-bang in Lab 6.
• Add turnPid(-10) on another button and make sure it comes back to where it started
• Challenge: use the test bed's 10-turn knob from Lab 5 to set the target, so the wheel follows the knob turn for turn

Check yourself#