EasyProfiledPID
Profiled PID Controller
A profiled PID controller combines a motion profile with feedback control.
Instead of sending the final target directly to a PID controller, it generates intermediate position and velocity setpoints that follow configured motion constraints. The PID controller then calculates an output to track the intermediate position.
EasyStack's EasyProfiledPID combines TrapezoidProfileEx with EasyPIDControllerEx, allowing you to configure the desired motion and feedback gains in one controller.
Creating a Profiled PID Controller
To create an EasyProfiledPID, provide the PID gains and motion constraints:
EasyProfiledPid controller
new EasyProfiledPid(kP, kl, kD, constraints);The parameters are:
| Parameter | Description |
|---|---|
kP | Proportional gain. |
kI | Integral gain. |
kD | Derivative gain. |
constraints | Maximum velocity and acceleration for the motion profile. |
For example, using meters for position:
TrapezoidProfileEx.Constraints constraints =
new TrapezoidProfileEx.Constraints(
0.5,
1.0
);
EasyProfiledPid controller =
new EasyProfiledPid(
8.0,
0.0,
0.5,
constraints
);The intended constraints are:
| Constraint | Value |
|---|---|
| Maximum velocity | 0.5 m/s |
| Maximum acceleration | 1.0 m/s² |
The gains and constraints are illustrative. Choose values appropriate for your mechanism.
Custom Calculation Period
The default calculation period is 0.02 seconds.
To configure another period:
EasyProfiledPid controller =
new EasyProfiledPid(
8.0,
0.0,
0.5,
constraints,
0.01
);This configures a 10 millisecond period.
The profile and PID controller both use this fixed period. They do not measure elapsed time or schedule updates automatically.
Call calculate() at approximately the configured interval.
Goal and Setpoint
The controller maintains two different states:
| State | Meaning |
|---|---|
| Goal | The destination requested by your program. |
| Setpoint | The intermediate position and velocity generated by the profile. |
For example, your goal might be a height of 1.0 meter while the current profile setpoint is only 0.2 meters.
The PID tracks that intermediate setpoint as it progresses toward the goal.
A mechanism can therefore be tracking its setpoint correctly while still being far from its final goal.
Initializing from the Mechanism's State
Before starting control, initialize the profile using the actual mechanism position:
controller.reset(measuredPosition);This assumes the mechanism's initial velocity is zero.
If it is already moving, provide its measured velocity:
controller.reset(
measuredPosition,
measuredVelocity
);For example:
controller.reset(0.25, 0.1);With meter-based units, this starts the profile from a height of 0.25 meters and an upward velocity of 0.1 m/s.
This avoids starting the reference from an unrelated initial state.
Do not reset every loop. Doing so repeatedly replaces the profile's progress and clears PID history.
Setting a Position Goal
Use setGoal():
controller.setGoal(1.0);The numeric overload creates a goal with zero velocity:
controller.setGoal(
new 0.0)The position uses the same units as the measurement.
For an angular mechanism using radians:
controller.setGoal(Math.toRadians (90.O));Setting a goal does not immediately advance the profile or calculate motor output.
Setting a Position and Velocity Goal
To specify both values, provide a TrapezoidProfileEx.State:
controller.setGoal(
new TrapezoidProfileEx.State(
targetPosition,
targetVelocity
)
);For example:
controller.setGoal(
new 0.2)This requests a goal position of 1.0 and a goal velocity of 0.2 in the corresponding units.
A nonzero goal velocity describes a moving endpoint. Use zero when the intended destination is stationary.
Reading the Goal
Use getGoal():
TrapezoidProfileEx.State goal controller.getGoal();
double targetPosition goal.position;
double targetVelocity goal.velocity;Calculating the Output
Call calculate() with the current position:
= controller.calculate(measuredPosition);Each call advances the profile by one configured period:
m_setpoint = m_profile.calculate(
getPeriod(),
m_setpoint,
m_goal
);It then calculates PID output using the profile's position:
output = m_controller.calculate(
measurement,
m_setpoint.position
);The profile advances from its previous reference state, rather than being restarted from the measurement each loop.
If the mechanism falls behind, the reference can continue moving. Choose constraints the mechanism can realistically follow.
The returned value is PID output only. This class does not automatically add feedforward or apply motor power.
Providing the Goal During Calculation
You can supply a position goal directly:
double output = controller.calculate(
measuredPosition,
targetPosition
);This creates a goal with zero velocity.
To supply a full goal state:
double output = controller.calculate(
measuredPosition,
new TrapezoidProfileEx.State(
targetPosition,
targetVelocity
)
);Both overloads store the goal and perform one profile and PID update.
You can also provide new constraints:
double output = controller.calculate(
measuredPosition,
goal,
constraints
);This replaces the constraints before calculating.
Call only one calculation overload per control cycle. Each call advances the profile by another period.
Reading the Profile Setpoint
Use getSetpoint():
TrapezoidProfileEx.State setpoint
controller.getSetpoint();The state contains:
= setpoint.position;
= setpoint.velocity;Read it after calculate() to obtain the reference used for that update:
double output = controller.calculate(measuredPosition);
TrapezoidProfileEx.State reference =
controller.getSetpoint();
telemetry.addData("Reference Position", reference.position);
telemetry.addData("Reference Velocity", reference.velocity);The velocity is the profile's requested velocity, not a measured mechanism velocity.
Updating Motion Constraints
Use setConstraints():
controller.setConstraints(
new TrapezoidProfileEx.Constraints(
0.3,
0.6
)
);This replaces the profile object while preserving the stored goal, current profile setpoint, and PID history.
The updated constraints are used on subsequent calculations.
To retrieve the current constraints:
TrapezoidProfileEx.Constraints Constraints
controller.getConstraints();Motion constraints govern the generated reference. They do not directly limit motor power or guarantee that actual motion remains within those limits.
Updating the PID Gains
Use setPID():
controller.setPID(10.0, 0.0, 0.6);Or update individual gains:
controller.setP(10.0);
controller.
controller.setD(0.6);Read them using:
double = controller.getP();
double kl = controller.getl();
double — controller.getD();These methods delegate to the internal EasyPIDControllerEx.
Updating gains preserves the profile state and PID history. The setters return void, so they cannot be chained.
Limiting the Integral Contribution
Use setIntegratorRange():controller.setIntegratorRange(-0.2, 0.2);For a positive integral gain, this limits the integral contribution to the PID output.
The default range is -1.0 to 1.0.
This does not limit the total output or change the motion constraints.
To read the accumulator before multiplication by kI:
double accumulatedError
controller.getAccumulatedError();The integral accumulates tracking error relative to the intermediate profile position, not directly to the final goal.
Integral Zone
Use setIZone() to restrict integral action to a region near the profile setpoint:
controller.setIZone(0.1);For position in meters, this allows integration while the absolute tracking error is at most 0.1 meters. Larger errors clear the accumulated integral.
To retrieve the zone:
= controller.getIZone();The default is positive infinity.
To remove a finite zone:
controller.setIZone(Double.POSITIVE_INFINITY);Negative values are rejected by the internal PID controller.
Setting Tolerances
Use setTolerance():
controller.setTolerance(0.01);For meter-based position, this requires an absolute tracking error below 0.01 meters.
To also constrain the rate of change of tracking error:
controller.setTolerance(0.01, 0.05);The parameters are:
| Parameter | Description |
|---|---|
positionTolerance | Allowed error between the profile position and measurement. |
velocityTolerance | Allowed rate of change of that tracking error. |
Despite its name, velocityTolerance does not directly compare measured velocity with the final goal velocity. It checks the internal PID's error derivative.
The defaults are:
| Setting | Default |
|---|---|
| Position tolerance | 0.05 |
| Error-derivative tolerance | Positive infinity |
The comparisons are strict. An error equal to its tolerance does not pass.
Read the configured values using:
double positionTolerance
controller.getPositionTolerance();
double velocityTolerance
controller.getVelocityTolerance();Checking the Current Setpoint
Use atSetpoint():
= controller.atSetpoint();This delegates to the internal PID controller and checks whether the latest tracking error and error derivative are within tolerance.
It can return true while the profile is still moving toward its goal.
Check it after calculating with a fresh measurement:
double output = controller.calculate(measuredPosition);
= controller.atSetpoint();Checking the Final Goal
Use atGoal():
= controller.atGoal();The implementation requires both:
atSetpoint()and:
m_goal.equals(m_setpoint)This distinguishes successful reference tracking from reaching the profile's destination.
The state comparison depends on the equals() implementation in TrapezoidProfileEx.State. This class does not apply an additional tolerance to that comparison.
Neither atSetpoint() nor atGoal() stops the motor or forces the output to zero.
Reading Tracking Errors
Use:
double positionError = controller.getPositionError();
= controller.getVelocityError();The position error is based on:
profilePosition measuredPositionwith wrapping applied when continuous input is enabled.
The velocity error accessor returns the rate of change of that error, as calculated by the internal PID. It is not an independently measured velocity error.
Continuous Input
For an angular mechanism that can cross a wrap boundary, enable continuous input:
controller.enableContinuousInput(-Math.PI, Math.PI);For degrees:
controller.enableContinuousInput (-180.O, 180.0);The controller adjusts the goal and profile position to nearby equivalent positions relative to the current measurement. The internal PID also wraps its error.
For example, a target of -179° from a measurement of 179° can be approached across the nearby wrap boundary.
The stored goal may be represented as an equivalent value outside the original interval, such as 181°.
To disable continuous input:
controller.disableContinuousInput();Use it only when crossing the boundary is mechanically valid. It does not enforce cable limits or physical stops.
Working with State Objects
This implementation stores state objects by reference:
controller.setGoal(goalState);
controller.reset(measuredState);It also returns internal state references through getGoal() and getSetpoint().
Treat these objects as controller-owned after passing them in. In particular, continuous-input calculations can modify their position fields.
To keep an independent snapshot, copy the values:
TrapezoidProfileEx.State current =
controller.getSetpoint();
TrapezoidProfileEx.State snapshot =
new TrapezoidProfileEx.State(
current.position,
current.velocity
);Use setConstraints() when changing constraints so the profile is reconstructed consistently.
Combining with Feedforward
The PID output can be combined with feedforward based on the generated reference.
For an elevator:
double pidVoltage = controller.calculate(measuredPosition);
TrapezoidProfileEx.State reference
controller.getSetpoint();
double feedforwardVoltage
feedforward.calculate(reference.velocity);
double requestedVoltage pidVoltage + feedforwardVoltage;Here, feedforward is an EasyElevatorFeedforward, and both outputs must use volts.
To include acceleration compensation, estimate reference acceleration from consecutive reference velocities:
double previousVelocity
controller.getSetpoint().velocity;
= controller.calculate(measuredPosition);
TrapezoidProfileEx.State reference
controller.getSetpoint();
double referenceAcceleration —
previousVelocity)
/ controller.getPeriod();
double feedforwardVoltage feedforward.calculate(
referenceAcceleration
double requestedVoltage pidVoltage + feedforwardVoltage;This is the average reference acceleration over that update.
Limiting the Output
Motion constraints do not clamp the calculated output.
For direct FTC motor power, apply a final limit:
double output = controller.calculate(measuredPosition);
double power = Math.max(-0.5, Math.min(0.5, output));
motor.setPower(power);If your PID and feedforward produce volts, limit and convert the combined voltage before passing it to setPower().
Resetting the Controller
The available reset methods are:
controller.reset(measuredPosition);
controller.reset(measuredPosition, measuredVelocity);
controller.reset(
new TrapezoidProfileEx.State(
measuredPosition,
measuredVelocity
)
);Each resets the internal PID and assigns the supplied state as the profile's starting setpoint.
The position-only overload assumes zero velocity.
Reset preserves the goal, gains, constraints, period, tolerances, and continuous-input configuration.
For a new movement:
controller.reset(measuredPosition, measuredVelocity);
controller.setGoal(targetPosition);After resetting, completion checks remain false until the internal PID receives another measurement through calculation.
Resetting does not command a motor to stop.
Complete Example
The following helper controls encoder position with a profiled reference.
It uses ticks for position, ticks per second for velocity, and ticks per second squared for acceleration. The PID gains produce normalized motor power.
Call update() approximately every 20 milliseconds. The helper does not schedule itself.
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotor;
import
dev.easyfamily.easystack.control.wpiLibControllers.EasyProfiledPi
d;
import dev.easyfamily.easystack.trajectory.TrapezoidProfileEx;
public class ProfiledPositionControl {
private final DcMotorEx motor;
private final EasyProfiledPid controller;
public ProfiledPositionControl(DcMotorEx motor) {
this.motor = motor;
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // Illustrative gains and constraints.
controller = new EasyProfiledPid(
0.005,
0.0,
0.0002,
new TrapezoidProfileEx.Constraints(
500.0,
1000.0
),
0.02
);
controller.setTolerance(10.0, 25.0);
}
public void start(double targetTicks) {
controller.reset(
motor.getCurrentPosition(),
motor.getVelocity()
); controller.setGoal(targetTicks);
}
public void setGoal(double targetTicks) {
controller.setGoal(targetTicks);
}
// Call approximately once every 0.02 seconds.
public void update() {
double output = controller.calculate(
motor.getCurrentPosition()
);
double power = Math.max(
-0.5,
Math.min(0.5, output)
);
motor.setPower(power); }
public boolean atGoal() {
return controller.atGoal();
}
public double getReferencePosition() {
return controller.getSetpoint().position;
}
public double getReferenceVelocity() {
return controller.getSetpoint().velocity;
}
public void stop() {
motor.setPower(0.0);
controller.reset(
motor.getCurrentPosition(),
motor.getVelocity()Create the helper and start a movement:
ProfiledPositionControl mechanism —
new ProfiledPositionControl(motor);
mechanism.start(1000.0);Inside the configured periodic loop:
mechanism.update();
telemetry.addData(
"Reference Position",
mechanism.getReferencePosition()
);
telemetry.addData(
"Reference Velocity",
mechanism.getReferenceVelocity()
);
telemetry.addData("At Goal", mechanism.atGoal());
telemetry.update();When control ends, stop calling update() and command:
mechanism.stop();The example uses the existing encoder reference. Establish that reference before selecting absolute goals, and configure positive motor power to increase the measured position.
Last updated 2026-11-16