EasyPIDFController
PIDF Controller
A PIDF controller combines feedback control with a feedforward term.
The PID portion corrects the difference between the target and the current measurement. Feedforward adds an output based on the target itself, without waiting for an error to develop.
EasyStack's EasyPIDFController extends EasyPIDController and adds a feedforward term proportional to the target:
feedforward kF * target;This can be useful for velocity control, where maintaining a higher target speed generally requires a higher output.
Creating a PIDF Controller
To create an EasyPIDFController, provide the proportional, integral, derivative, and feedforward gains:
EasyPIDFController controller —
new EasyPIDFController(kP, kl, kD, kF);The parameters are:
| Parameter | Description |
|---|---|
| kP | Proportional gain. Responds to the current error. |
| kI | Integral gain. Responds to accumulated error over time. |
| kD | Derivative gain. Responds to changes in the measurement. |
| kF | Feedforward gain. Multiplies the target to produce an additional output. |
For example:
EasyPIDFController controller =
new EasyPIDFController(
0.0002,
0.0,
0.0,
0.00015
);These gains are illustrative and must be tuned for your mechanism and measurement units.
You can also create the controller using an existing EasyPIDFCoefficients object:
EasyPIDFController controller —
new EasyPIDFController(coefficients);The constructor copies the object's kP, kI, kD, and kF values.
Setting the Target
Use the inherited setTargetPosition() method:
controller.setTargetPosition(3000.O);Although the method is named setTargetPosition(), the calculation can operate on another measured quantity, such as velocity. The target and measurement must represent the same quantity and use the same units.
For example, when controlling velocity in ticks per second:
controller.setTargetPosition(3000.0);
double output = controller.calculate(
motor.getVelocity()
);Here, motor is an FTC DcMotorEx, whose no-argument getVelocity() returns encoder ticks per second.
To retrieve the target:
= controller.getTargetPosition();The default target is 0.0.
Calculating the Output
Use calculate() with the current measurement:
double output = controller.calculate(currentMeasurement);The controller first calculates the inherited PID output:
double pidOutput super.calculate(currentMeasurement);It then adds feedforward:
double feedforward kF * getTargetPosition();The final output is:
output pidOutput + feedforward;calculate() returns this value without applying it to a motor.
Providing the Target During Calculation
The inherited two-argument overload is also available:
double output = controller.calculate(
currentMeasurement,
target
);For example:
double output = controller.calculate(
motor.getVelocity(),
3000.0
);This updates the stored target and calls the PIDF calculation, including feedforward.
It is equivalent to:
controller.setTargetPosition(3000.0);
double output = controller.calculate(
motor.getVelocity()
);How Feedforward Works
Feedforward is calculated directly from the target:
feedforward kF * target;For example, with:
controller.setF(0.00015);
controller.setTargetPosition(3000.O);the feedforward contribution is:
0.00015 * 0.45If the PID portion produces 0.10, the combined output is:
0.10 + 0.45 0.55For a positive kF, feedforward follows the sign of the target:
| Target | Feedforward with kF = 0.00015 |
|---|---|
| 3000 | 0.45 |
| 1000 | 0.15 |
| 0 | 0.0 |
| -1000 | -0.15 |
Unlike an error-based correction, feedforward can remain nonzero when the measurement reaches the target. For velocity control, that output can help maintain the requested speed.
Feedforward Units
The units of kF depend on the target and output units.
If the target is in ticks per second and the output is motor power:
kF = motorPower / targetVelocity;For example, if testing shows that a steady power of 0.45 maintains approximately 3000 ticks per second:
0.45 / 300.0;This gives an initial estimate of:
0.00015;This estimate assumes an approximately proportional relationship between speed and required power. Actual behavior can vary with load and battery voltage.
Position Control
When the target represents position, this implementation calculates:
feedforward kF * targetPosition;It does not automatically calculate gravity compensation, static friction compensation, or acceleration feedforward.
For example, it does not implement an arm model such as:
gravityCompensation — kG * Math.cos(angle);Choose kF based on the meaning of your target. A target-proportional feedforward term is not automatically an appropriate holding output for every position-controlled mechanism.
Updating the Feedforward Gain
Use setF() to change only kF:
controller.setF(O. 00018);The PID gains remain unchanged.
To disable feedforward:
controller.setF(O.O);The controller then returns only the inherited PID output.
Changing kF does not reset the PID state.
Updating All Gains
Use setPIDF() to update all four gains:
controller.setPIDF(
0.00025,
0.0,
0.0,
0.00016
);You can also supply an existing EasyPIDFCoefficients object:
controller.setPIDF(coefficients);This copies the coefficient values into the controller.
To update only the PID gains, use the inherited setPID() method:
controller.setPID(
0.00025,
0.0,
0.0
);This leaves kF unchanged.
Updating gains does not clear the accumulated integral or measurement history. Call reset() separately when you need to clear that state.
PID Timing and Derivative Behavior
The PID portion retains the behavior of EasyPIDController.
Elapsed time is measured automatically using System.nanoTime(), and the derivative is calculated from the negative rate of change of the measurement:
derivative =
-(currentMeasurement - previousMeasurement) / dt;On the first calculation after construction or reset, the PID portion does not accumulate integral error or calculate a measurement rate. Feedforward is still added on that first call.
A target change does not directly create a derivative spike, but it can immediately change both the proportional output and feedforward output.
Limiting the Integral
Use the inherited setMaxIntegralSum() method:
controller.setMaxIntegralSum(200.O);This limits the accumulated integral to the range:
-200.0 to 200.0The limit applies before multiplication by kI.
It does not limit feedforward or the total output.
The default value of 0.0 means the integral is not clamped.
Setting a Tolerance
Use setTolerance() to configure the allowed error:
controller.setTolerance(50.0);The tolerance uses the same units as the target and measurement.
For velocity measured in ticks per second, this allows an error of up to 50 ticks per second.
Check the latest error using:
double output = controller.calculate(
motor.getVelocity()
);
boolean atSpeed = controller.atTarget();The inherited check is:
Math.abs(error) toleranceatTarget() does not disable feedforward or force the output to zero.
Call calculate() before checking it, because it uses the stored error. Immediately after construction or reset, that error is zero until a new calculation updates it.
Reading the Error
Use the inherited getError() method:
double error = controller.getError();For example:
double output = controller.calculate(
motor.getVelocity()
);
telemetry.addData("Target", controller.getTargetPosition());
telemetry.addData("Error", controller.getError());
telemetry.addData("At Target", controller.atTarget());The error is the target minus the most recent measurement. Feedforward does not change how this error is calculated.
Limiting the Output
EasyPIDFController does not clamp the combined PID and feedforward output.
If the result is used as FTC motor power, apply a final limit:
double output = controller.calculate(
motor.getVelocity()
);
double power = Math.max(-1.0, Math.min(1.0, output));
motor.setPower(power);To restrict the output further:
double maxPower = 0.75;
double power = Math.max(
-maxPower,
Math.min(maxPower, output)
);
motor.setPower(power);This limits the final applied power, including feedforward.
Setting a zero target makes feedforward zero, but the PID portion may still produce an output while the mechanism is moving or while integral remains accumulated. To disable the mechanism, explicitly command zero motor power and stop applying controller output.
Resetting the Controller
Use the inherited reset() method:
controller.reset();This clears the PID history, including accumulated integral, stored error, previous measurement, and timing state.
It preserves:
PID gains.
Feedforward gain.
Target.
Tolerance.
Integral limit.
For example:
controller.reset();
controller.setTargetPosition(3000.O);The next calculation starts with cleared PID history and still includes feedforward.
Resetting alone does not stop the motor:
motor.setPower(0.0);
controller.reset();Chaining Configuration Methods
setF() and setPIDF() return an EasyPIDFController, so they support chaining:
controller
.setPIDF(0.0002, 0.0, 0.0, 0.00015)
.setF(0.00016);Inherited configuration methods such as setTolerance() return EasyPIDController. Call PIDF-specific methods before them, or use separate statements:
EasyPIDFController controller =
new EasyPIDFController(
0.0002,
0.0,
0.0,
0.00015
);
controller.setF(0.00016);
controller.setTolerance(50.0);
controller.setMaxIntegralSum(200.0);Complete Example
The following FTC example controls motor velocity with EasyPIDFController.
Press A to enable a target of 3000 ticks per second. Press B to disable the controller and command zero motor power.
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import dev.easyfamily.easystack.control.EasyPIDFController;
@TeleOp(name = "EasyPIDF Velocity Example")
public class EasyPIDFVelocityExample extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
DcMotorEx motor = hardwareMap.get(
DcMotorEx.class,
"velocityMotor"
);
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // Illustrative gains: tune for your mechanism.
EasyPIDFController controller =
new EasyPIDFController(
0.0002,
0.0,
0.0,
0.00015
);
controller.setTolerance(50.0);
boolean enabled = false;
telemetry.addLine("A: Enable at 3000 ticks/s");
telemetry.addLine("B: Disable");
telemetry.update();
waitForStart(); try {
while (opModeIsActive()) {
if (gamepad1.b) {
enabled = false;
} else if (gamepad1.a && !enabled) {
controller.reset();
controller.setTargetPosition(3000.0);
enabled = true;
}
double measuredVelocity = motor.getVelocity();
double power = 0.0;
if (enabled) {
double output = controller.calculate(
measuredVelocity
);
power = Math.max(
-1.0, Math.min(1.0, output)
);
}
motor.setPower(power);
telemetry.addData("Enabled", enabled);
telemetry.addData(
"Velocity (ticks/s)",
measuredVelocity
);
telemetry.addData("Power", power);
if (enabled) {
telemetry.addData(
"Target (ticks/s)",
controller.getTargetPosition()
);
telemetry.addData(
"Error (ticks/s)", controller.getError()
);
telemetry.addData(
"At Speed",
controller.atTarget()
);
}
telemetry.update();
idle();
}
} finally {
motor.setPower(0.0);
}
}
}RUN_WITHOUT_ENCODER allows this controller to calculate the motor power while encoder velocity remains available for feedback.
Configure the motor so that positive power produces a positive measured velocity. Keep all targets, measurements, and gains consistent with ticks per second.
Last updated 2026-11-16