EasyStack Docs

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:

Java
EasyProfiledPid controller
        new EasyProfiledPid(kP, kl, kD, constraints);

The parameters are:

ParameterDescription
kPProportional gain.
kIIntegral gain.
kDDerivative gain.
constraintsMaximum velocity and acceleration for the motion profile.

For example, using meters for position:

Java
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:

ConstraintValue
Maximum velocity0.5 m/s
Maximum acceleration1.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:

Java
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:

StateMeaning
GoalThe destination requested by your program.
SetpointThe 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:

Java
controller.reset(measuredPosition);

This assumes the mechanism's initial velocity is zero.

If it is already moving, provide its measured velocity:

Java
controller.reset(
        measuredPosition,
        measuredVelocity
);

For example:

Java
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():

Java
controller.setGoal(1.0);

The numeric overload creates a goal with zero velocity:

Java
controller.setGoal(
         new 0.0)

The position uses the same units as the measurement.

For an angular mechanism using radians:

Java
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:

Java
controller.setGoal(
        new TrapezoidProfileEx.State(
                targetPosition,
                targetVelocity
        )
);

For example:

Java
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():

Java
TrapezoidProfileEx.State goal controller.getGoal();
double targetPosition goal.position;
double targetVelocity goal.velocity;

Calculating the Output

Call calculate() with the current position:

Java
 = controller.calculate(measuredPosition);

Each call advances the profile by one configured period:

Java
m_setpoint = m_profile.calculate(
        getPeriod(),
        m_setpoint,
        m_goal
);

It then calculates PID output using the profile's position:

Java
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:

Java
double output = controller.calculate(
        measuredPosition,
        targetPosition
);

This creates a goal with zero velocity.

To supply a full goal state:

Java
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:

Java
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():

Java
TrapezoidProfileEx.State setpoint
        controller.getSetpoint();

The state contains:

Java
 = setpoint.position;
 = setpoint.velocity;

Read it after calculate() to obtain the reference used for that update:

Java
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():

Java
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:

Java
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():

Java
controller.setPID(10.0, 0.0, 0.6);

Or update individual gains:

Java
controller.setP(10.0);
controller.
controller.setD(0.6);

Read them using:

Java
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

Java
Use setIntegratorRange():
Java
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:

Java
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:

Java
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:

Java
 = controller.getIZone();

The default is positive infinity.

To remove a finite zone:

Java
controller.setIZone(Double.POSITIVE_INFINITY);

Negative values are rejected by the internal PID controller.

Setting Tolerances

Use setTolerance():

Java
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:

Java
controller.setTolerance(0.01, 0.05);

The parameters are:

ParameterDescription
positionToleranceAllowed error between the profile position and measurement.
velocityToleranceAllowed 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:

SettingDefault
Position tolerance0.05
Error-derivative tolerancePositive infinity

The comparisons are strict. An error equal to its tolerance does not pass.

Read the configured values using:

Java
double positionTolerance
        controller.getPositionTolerance();

double velocityTolerance
        controller.getVelocityTolerance();

Checking the Current Setpoint

Use atSetpoint():

Java
 = 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:

Java
double output = controller.calculate(measuredPosition);
 = controller.atSetpoint();

Checking the Final Goal

Use atGoal():

Java
 = controller.atGoal();

The implementation requires both:

Java
atSetpoint()

and:

Java
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:

Java
double positionError = controller.getPositionError();
 = controller.getVelocityError();

The position error is based on:

Java
profilePosition measuredPosition

with 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:

Java
controller.enableContinuousInput(-Math.PI, Math.PI);

For degrees:

Java
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:

Java
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:

Java
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:

Java
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:

Java
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:

Java
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:

Java
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:

Java
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:

Java
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.

Java
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);
Java
        // 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()
        );
Java
        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);
Java
    }

    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()
Java

Create the helper and start a movement:

Java
ProfiledPositionControl mechanism —
        new ProfiledPositionControl(motor);
mechanism.start(1000.0);

Inside the configured periodic loop:

Java
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:

Java
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

EasyStack - FTC Java library

This site records page views of visitors to understand how the documentation is used. Nothing is sold or shared with third parties.