Feedforward and Motion Profiling

Feedforward: predict, don't just react

Where PID reacts to error, feedforward predicts the output a mechanism needs to follow a desired motion — before any error appears. Combining feedforward (the bulk of the output) with PID (small corrections) gives the best of both: responsive and accurate.

WPILib provides feedforward classes:

• SimpleMotorFeedforward — for flywheels and drivetrains (no gravity load):

  SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kS, kV, kA);
  double volts = ff.calculate(targetVelocity);
  motor.setVoltage(volts);
  

• ArmFeedforward — adds a kG (gravity) term that depends on arm angle.
• ElevatorFeedforward — adds a constant kG to fight gravity on an elevator.

The gains mean:

• kS — voltage to overcome static friction.
• kV — voltage per unit of velocity (the big one).
• kA — voltage per unit of acceleration.
• kG — voltage to hold against gravity (arm/elevator only).

You find these with the SysId characterization tool. (API note: as of WPILib 2025 the two-argument calculate(velocity, acceleration) overload is deprecated in favor of calculateWithVelocities(currentVel, nextVel), which discretizes correctly; the single-argument calculate(velocity) form is not deprecated and is fine for learning.)

Combining feedforward and PID

The combination is simply adding their outputs:

double output = ff.calculate(targetVel) + pid.calculate(measuredVel, targetVel);
motor.setVoltage(output);

Feedforward does most of the work; PID cleans up the rest.

Motion profiling: smooth setpoints

Feeding a mechanism a giant instant jump ("go from 0 to 90 degrees NOW") causes a violent move. A motion profile generates a smooth path of intermediate setpoints over time. WPILib's TrapezoidProfile ramps velocity up, cruises, then ramps down — a trapezoid shape — bounded by max velocity and max acceleration constraints.

ProfiledPIDController: the convenient combo

Combining a profile with PID is so common that WPILib bundles them into ProfiledPIDController: it internally generates trapezoid setpoints and runs PID to follow them.

ProfiledPIDController controller = new ProfiledPIDController(
    kP, kI, kD,
    new TrapezoidProfile.Constraints(maxVel, maxAccel));

double output = controller.calculate(encoder.getPosition(), goalPosition)
              + feedforward.calculate(controller.getSetpoint().velocity);
motor.setVoltage(output);

This is the gold-standard pattern for an arm or elevator: a profile for smoothness, feedforward for prediction, PID for accuracy.

Why this matters for competition

Well-tuned feedforward + motion profiling makes mechanisms fast and repeatable — an arm that snaps to scoring position without overshoot, a flywheel that holds RPM through a shot. The difference between "it kind of works" and "it works every time" is usually here. The same profiling idea scales up to whole-robot paths, which is the next module.