Combining PID and Feedforward

Hook

In the last two lessons you built two weapons:

1. Feedforward: computes the right output from physics. Fast and predictive, but can’t react to surprises.
2. PID: reacts to sensor error. Handles anything feedforward misses, but is always one step behind — it waits for error to develop before correcting.

Neither alone is the full story. Pure feedforward is like a skilled chef who measures every ingredient perfectly — except when someone swaps the salt for sugar (a disturbance). Pure PID is like an amateur who only corrects when they taste something wrong — always a bit late, and spending too much effort on problems they could have anticipated.

The pro move: combine them. Feedforward handles the expected. PID handles the unexpected.

Core concept

Why FF alone fails

Feedforward is based on a model: “I know this mechanism needs approximately X volts to run at Y RPM.” But models are never perfect:

• Battery sag: as the battery discharges during a match, the same voltage command produces less current. FF doesn’t know the battery dropped from 12.5 V to 11.8 V.
• Mechanical wear: friction changes over time. A gearbox that was characterized in week 1 may have more friction by week 6.
• Load disturbances: a game piece entering a shooter mechanism adds inertia. FF doesn’t know the piece is there.
• Manufacturing variance: two robots built to identical specs will have slightly different kV values. FF constants from one won’t perfectly apply to another.

Without feedback, these disturbances accumulate as steady errors. The flywheel that ran at exactly 3500 RPM in characterization might run at 3420 RPM in competition.

Why PID alone is slow

PID can only act after error has already developed. For a step change in setpoint (e.g., flywheel spinning up from 0 to 3500 RPM):

1. Loop 1: error = 3500 RPM. PID output is large. But “large” is relative to kP — if kP is small enough to avoid oscillation, the output on the first loop is still modest.
2. Loop 10: flywheel is at 1000 RPM. Error = 2500 RPM. PID is still pushing hard.
3. Loop 50: flywheel at 2800 RPM. Error = 700 RPM. Output is now modest.

The spin-up takes dozens of loop cycles. Meanwhile, the robot might have already moved and fired. For a shooter, slow spin-up means inconsistent shots.

The PID gains that give fast spin-up tend to cause oscillation around the setpoint. So teams end up with a tradeoff: either fast but oscillating, or stable but slow.

Feedforward breaks that tradeoff. With FF supplying 90% of the required output from loop 1, the PID only needs to add a small trim — and small PID gains don’t oscillate.

The combined architecture

              desired setpoint
                     |
          +----------+----------+
          |                     |
    [Feedforward]          [PID Controller]
    (kS, kV, kA, kG)       (kP, kI, kD)
          |                     |
          |   measurement ------>|
          |                     |
          +----------+----------+
                     |
                  [sum]
                     |
               motor output

In code, this is just an addition:

double ffOutput = feedforward.calculate(desiredVelocity);
double pidOutput = pidController.calculate(measurement, desiredVelocity);
double totalOutput = ffOutput + pidOutput;
motor.setVoltage(totalOutput);

The PID controller’s gains can now be much smaller than if PID were doing the work alone, because FF handles the large steady-state requirement. Smaller PID gains mean less oscillation risk.

Practical example 1: flywheel velocity controller

public class ShooterSubsystem extends SubsystemBase {
    private final CANSparkMax motor = new CANSparkMax(1, MotorType.kBrushless);
    private final RelativeEncoder encoder = motor.getEncoder();

    // FF characterized via SysId: kS=0.20V, kV=0.00220V/(rot/s), kA=0.010V/(rot/s²)
    // Note: WPILib uses rotations/second, not RPM
    private final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0.20, 0.00220, 0.010);

    // PID with small gains — FF does the heavy lifting
    private final PIDController pid = new PIDController(0.0001, 0.000002, 0.0);

    private double targetRPM = 0.0;

    public ShooterSubsystem() {
        pid.setTolerance(50.0); // within 50 RPM = at setpoint
    }

    public void setTargetRPM(double rpm) {
        if (Math.abs(rpm - targetRPM) > 200.0) {
            pid.reset(); // clear integrator on large setpoint changes
        }
        targetRPM = rpm;
    }

    public boolean isReady() {
        return pid.atSetpoint();
    }

    @Override
    public void periodic() {
        double currentRPM = encoder.getVelocity(); // REV returns RPM

        // Convert RPM to rotations/second for FF (WPILib FF units)
        double targetRPS = targetRPM / 60.0;
        double currentRPS = currentRPM / 60.0;

        double ffVolts = ff.calculate(targetRPS);           // predicts needed voltage
        double pidVolts = pid.calculate(currentRPS, targetRPS) * 12.0; // scale PID to volts

        double totalVolts = ffVolts + pidVolts;
        motor.setVoltage(totalVolts);

        // Log for tuning visualization
        SmartDashboard.putNumber("Shooter/TargetRPM", targetRPM);
        SmartDashboard.putNumber("Shooter/CurrentRPM", currentRPM);
        SmartDashboard.putNumber("Shooter/FFVolts", ffVolts);
        SmartDashboard.putNumber("Shooter/PIDVolts", pidVolts);
    }
}

Notice that the PID kP (0.0001) is tiny compared to what you’d need for a PID-only controller. The FF does the heavy lifting; the PID just corrects the 30-50 RPM error that remains.

Practical example 2: drivetrain velocity control

For a differential drivetrain following a path, each wheel has its own FF + PID loop:

public class DrivetrainSubsystem extends SubsystemBase {
    // Two FF controllers, one per side
    private final SimpleMotorFeedforward leftFF = new SimpleMotorFeedforward(kS, kV, kA);
    private final SimpleMotorFeedforward rightFF = new SimpleMotorFeedforward(kS, kV, kA);

    private final PIDController leftPID = new PIDController(0.5, 0.0, 0.0);
    private final PIDController rightPID = new PIDController(0.5, 0.0, 0.0);

    // In periodic() during autonomous path following:
    public void setWheelSpeeds(DifferentialDriveWheelSpeeds speeds) {
        double leftFF_V = leftFF.calculate(speeds.leftMetersPerSecond);
        double rightFF_V = rightFF.calculate(speeds.rightMetersPerSecond);

        double leftActual = leftEncoder.getRate();  // m/s
        double rightActual = rightEncoder.getRate();

        double leftPID_V = leftPID.calculate(leftActual, speeds.leftMetersPerSecond);
        double rightPID_V = rightPID.calculate(rightActual, speeds.rightMetersPerSecond);

        leftMotor.setVoltage(leftFF_V + leftPID_V);
        rightMotor.setVoltage(rightFF_V + rightPID_V);
    }
}

When each component handles what

Scenario	Handled by	Why
Initial spin-up from 0 RPM	FF (mostly)	FF immediately applies the right voltage; PID adds a small trim
Battery sags 0.5 V mid-match	PID	FF doesn’t know battery voltage changed; PID sees the RPM drop and increases output
Game piece enters the shooter	PID	The added inertia slows the flywheel; PID detects the error and corrects
Holding constant velocity at setpoint	FF (mostly)	kV * velocity provides the exact right voltage; PID’s contribution is near zero
Following a velocity trajectory	FF + PID	FF handles the predicted trajectory; PID corrects tracking error
Unexpected wheel slipping on drivetrain	PID	FF has no model for slip; PID reacts to the velocity error
Arm fighting gravity at horizontal position	FF (kG)	kG * cos(angle) perfectly compensates gravity; PID handles residual

Tuning the combined controller

The tuning sequence changes slightly when combining FF and PID:

1. Characterize FF first (use SysId or manual estimation).
2. Verify FF alone: command a constant setpoint with kP = kI = kD = 0. Does the mechanism reach approximately the right speed? Expect 5-15% error remaining.
3. Add P only: increase kP until the residual error is corrected without oscillation. kP will be much smaller than it would be for PID-only.
4. Add I if needed: for very precise applications (shooting distance matters significantly), a small kI eliminates the remaining few RPM of error.
5. kD is often not needed: with FF doing most of the work, the mechanism approaches setpoint smoothly. D is only needed if there’s still oscillation.

Common pitfall: mixing normalized and voltage units

A subtle bug: if your feedforward returns volts (e.g., 6.5 V) and your PID returns normalized output (-1 to 1), you can’t directly add them.

Wrong:

double ff = feedforward.calculate(targetRPS); // returns 6.5 V
double pid = pidController.calculate(measurement, setpoint); // returns -0.05 (normalized)
motor.setVoltage(ff + pid); // BUG: pid is not in volts

Fix option A — scale PID to volts:

double pidVolts = pidController.calculate(measurement, setpoint) * 12.0;
motor.setVoltage(ff + pidVolts);

Fix option B — work entirely in normalized units:

double ffNormalized = feedforward.calculate(targetRPS) / 12.0; // normalize FF
double pid = pidController.calculate(measurement, setpoint);   // normalized
motor.set(ffNormalized + pid);

Either works, but be consistent. Option A (working in volts) is preferred because it’s battery-voltage-independent.

Key takeaways

• FF alone can’t handle disturbances (battery changes, load changes, mechanical wear).
• PID alone is slow to respond to large step changes in setpoint, forcing a tradeoff between speed and stability.
• Combined FF + PID: FF handles the predictable, PID corrects the residual. output = FF + PID.
• PID gains can be much smaller in a combined system, reducing oscillation risk.
• Always use setVoltage() for FF-based systems; ensure both FF and PID outputs are in the same units before adding.
• Log FF and PID contributions separately to Shuffleboard/AdvantageScope during tuning so you can see each term’s contribution.

Common confusions

“My FF + PID controller still oscillates. Adding FF was supposed to fix that.”

If the PID gains are the same as before you added FF, they’re now too large — the PID is correcting a much smaller residual error with the same aggressive gains, causing it to overcorrect. Reduce kP by a factor of 5-10x when transitioning from PID-only to FF + PID.

“My FF is almost perfect but the mechanism always undershoots by the same amount.”

A constant residual offset is typically a kS issue — the friction compensation is slightly too low. Increase kS slightly. If the offset changes with velocity, kV may be slightly off. If it changes with mechanism angle (arm), kG may be off.

“My PID term is fighting my FF term — they seem to be opposing each other.”

Check the sign convention. If your encoder increases going forward, your motor positive direction is forward, and your setpoint is positive, then both FF and PID should produce positive output for a positive target. If they have opposite signs, one of your conventions is flipped.

“Which should I tune first at competition when I have 10 minutes?”

At competition with limited time: fix FF first (one test run to verify it gets approximately the right speed), then quickly tune kP to clean up residual error. Don’t touch kI or kD under time pressure — they can introduce new problems quickly.

Challenge

Code EditorJavaCtrl+Enter to run

▶ Run

Scenario 1 FF is 6.500 V—▶

Scenario 1 PID is 3.000 V—▶

Scenario 2 FF is 6.500 V—▶

Stuck? Show hint

FF: kS*sign(3000)+kV*3000 = 0.20 + 0.00210*3000 = 0.20 + 6.30 = 6.50 V. Scenario 1 PID: 0.001*(3000-0) = 3.0 V. Scenario 2 PID: 0.001*(3000-2960) = 0.04 V. Notice FF dominates in both cases.

⚡ Check your understanding

During a match, your flywheel is running at its 3500 RPM setpoint. Suddenly a ball is shot and the flywheel dips to 3350 RPM. Which component of the FF+PID controller primarily handles recovering the speed back to 3500 RPM?

AFeedforward — it increases output based on the new velocityBPID — it detects the error (150 RPM) and adds corrective outputCBoth equally — FF and PID split the recovery work 50/50DNeither — the flywheel recovers on its own due to motor back-EMF

Check →

What’s next

So far every controller has been given a target and told to reach it as fast as possible. But commanding a sudden jump in setpoint — from 0 to 60 degrees on an arm, for example — is physically harsh: the motor commands full power, the arm slams up, and the abrupt deceleration at the target stresses gears and structure. The next lesson introduces motion profiling: instead of commanding a step-change, you plan a smooth trajectory that respects the mechanism’s physical limits on velocity and acceleration.