How WPILib's TrapezoidProfile and ProfiledPIDController smooth mechanism motion, how to pick velocity and acceleration constraints, and when a profile beats plain PID.
to read
words
sections
A trapezoidal motion profile ramps a mechanism's velocity up to a max, holds it, then ramps back down to zero right as it hits the target, instead of a PID loop trying to slam straight to the setpoint the instant you ask for it. In WPILib that's edu.wpi.first.math.trajectory.TrapezoidProfile, and the class that does the useful part for you, generating that profile and running PID against it in one object, is edu.wpi.first.math.controller.ProfiledPIDController. Those are the package names as of the 2026 WPILib release; WPILib's own 2027 changelog already lists a rename of every Java package from edu.wpi.first to org.wpilib (and C++ frc:: to wpi::), so if your import doesn't resolve, check your installed WPILib version's docs before assuming the class moved or disappeared. If you've already got PID control and PID tuning down and your arm or elevator still lurches, overshoots, or trips your current limit on every move, this is almost always the fix.
A plain PIDController only knows one thing: how far you are from the setpoint right now. Set the setpoint to "elevator all the way up" and the P term immediately commands close to full output, because the error is huge. That's fine for a drivetrain correcting a small heading error, but on an elevator or arm it means a hard jerk at the start, a current spike that can trip your breaker or hit your smart current limit, and then overshoot and oscillation as the P term fights to stop exactly on target.
A motion profile fixes this by never handing the PID loop a setpoint that's far away. Instead, it hands the controller a sequence of intermediate setpoints, each one only a little farther along than the last, that ramp up to a maximum velocity, cruise, and ramp back down to zero velocity right at the goal. The PID loop is always chasing a nearby, reachable target, so the error stays small and the output stays smooth. For short moves that never reach max velocity, the profile just skips the cruise phase and the velocity graph looks like a triangle instead of a trapezoid, same idea either way.
TrapezoidProfile itself is simple. You build one with TrapezoidProfile.Constraints, which is just a max velocity and a max acceleration:
// 2 meters/second max velocity, 4 meters/second^2 max acceleration
var constraints = new TrapezoidProfile.Constraints(2.0, 4.0);
var profile = new TrapezoidProfile(constraints);
A TrapezoidProfile.State is a position-and-velocity pair, used for both where you are now and where you want to end up:
var current = new TrapezoidProfile.State(0.0, 0.0);
var goal = new TrapezoidProfile.State(1.5, 0.0); // 1.5 m, ending at rest
var setpoint = profile.calculate(0.02, current, goal); // state 20 ms in
Most teams never call calculate() on a raw TrapezoidProfile directly in a subsystem. It's the building block ProfiledPIDController uses internally.
ProfiledPIDController takes the same kP, kI, kD gains as a regular PID controller, plus a TrapezoidProfile.Constraints:
private final ProfiledPIDController controller = new ProfiledPIDController(
kP, kI, kD,
new TrapezoidProfile.Constraints(2.0, 4.0));
The workflow is different from plain PID in one important way: instead of calling calculate(measurement, setpoint) with the final target every loop, you call setGoal() once, and then just call calculate(measurement) every periodic cycle. Internally, the controller advances its own remembered setpoint one cycle closer to the goal each time, then runs PID between that new setpoint and your measurement. It's tracking from where it left off, not from where the mechanism actually is right now, which is exactly why the reset() gotcha below matters:
public void setGoal(double positionMeters) {
controller.setGoal(positionMeters);
}
@Override
public void periodic() {
double output = controller.calculate(encoder.getPosition());
motor.setVoltage(output);
}
atGoal() tells you when the mechanism has both reached the goal position and settled within the tolerance you set with setTolerance(). getSetpoint() returns the controller's current intermediate TrapezoidProfile.State, position and velocity, which matters for the next part.
A profile smooths the setpoint, but a P term still has to generate real output to hold an arm or elevator up against gravity, which means it's never actually at zero error and never really "settled." The fix is feedforward: ArmFeedforward or ElevatorFeedforward, added to the PID output, using the profile's own setpoint velocity so the feedforward and the profile always agree on what the mechanism should be doing right now:
private final ArmFeedforward feedforward = new ArmFeedforward(kS, kG, kV, kA);
@Override
public void periodic() {
double pidOutput = controller.calculate(encoder.getPosition());
var setpoint = controller.getSetpoint();
double ffOutput = feedforward.calculate(setpoint.position, setpoint.velocity);
motor.setVoltage(pidOutput + ffOutput);
}
With a good feedforward, the PID term only has to correct small tracking error instead of holding the whole mechanism against gravity, which is why teams that add profiling almost always add feedforward at the same time. See the elevator and arm design guide for how kG and gearing interact.
Don't guess these. Start from what the mechanism can actually do:
getSetpoint().position closely. If it lags noticeably, back the acceleration constraint down; if it tracks with room to spare, push it up.Tune kP last, after the profile and feedforward are doing most of the work. It should only need to correct small errors, not drive the whole move, which is exactly what makes profiled mechanisms so much easier to tune than a bare PID loop. The general tuning process from the PID tuning guide still applies once you're at that stage.
ProfiledPIDController carries its last computed setpoint state internally. If you set a goal, the robot gets disabled mid-move, and the arm sags under gravity while disabled, the controller's remembered setpoint no longer matches where the arm actually is. The first calculate() call after re-enabling can then command a large jump, because the controller thinks it's still where it left off. Call controller.reset(measuredPosition, measuredVelocity) with a real sensor reading whenever you start driving toward a new goal, typically in a command's initialize(), not just once at robot boot.
Profiling earns its keep on mechanisms with real mass and gravity to fight: elevators, arms, turrets, anything where a hard PID slam risks a current trip, a skipped chain, or stripped gears. It's less necessary for things like a shooter flywheel's velocity loop, where you're already commanding a steady-state RPM rather than moving between positions, or for small heading corrections on a drivetrain, where the error is already small enough that a plain PID loop rarely misbehaves. Full-field autonomous driving is a different problem entirely: PathPlanner and Choreo generate a complete 2D holonomic trajectory ahead of time, see PathPlanner and the Choreo vs PathPlanner comparison, rather than profiling one degree of freedom on the fly.
If you're already running CTRE or REV closed-loop hardware, check whether their onboard profiling covers your case before reaching for TrapezoidProfile. CTRE's Motion Magic runs a trapezoidal profile (with an optional jerk-limited S-curve, or the separate exponential-profile Motion Magic Expo variant) directly on the Talon FX, so the roboRIO just sends a target and the motor controller handles the rest without needing a setpoint streamed every 20 ms loop. REV's MAXMotion does the same on a SPARK MAX or SPARK Flex, regenerating the profile roughly every 10 ms with the underlying PID loop running at about 1 ms. Both are worth a look in the CTRE Phoenix ecosystem guide and REV Robotics ecosystem guide. WPILib also ships an ExponentialProfile class for voltage-limited motion as an alternative shape, but trapezoidal is what almost every team should reach for first.
Plain PID hands your loop a distant setpoint and lets it fight its way there. TrapezoidProfile and ProfiledPIDController hand it a nearby, physically reachable setpoint every cycle instead, which is why the same kP that caused a violent slam on a raw PIDController often looks calm and controlled once it's wrapped in a profile. Pick velocity and acceleration constraints from what the mechanism can actually do, add feedforward using the profile's own setpoint velocity, remember to reset() on re-enable, and tune kP last.
This article is AI-assisted: drafted from primary sources, then reviewed and edited by hand. Errors still get through. When one is reported we fix it and write down what changed — publicly, in the corrections log.
Keep going
You came here for one answer. These lessons teach the same subject properly, in the order a team actually learns it. All 51 are free and none of them need an account to read.
Go deeper
LearnFRC has 394 free FRC lessons across every department. Create a free account to save your place, track your progress, and earn a certificate.
Create a free accountKeep reading
Structured lessons and quizzes across every department. Create a free account to save your progress, track your team, and earn a certificate.