EasyPIDController
PID Controller
A PID controller uses feedback to calculate an output that moves a mechanism toward a target.
Instead of applying a fixed motor power, the controller adjusts its output based on the difference between the target and the current measurement.
EasyStack's EasyPIDController handles the PID calculations and measures the time between updates automatically. You provide the target and current measurement, then apply the calculated output to your mechanism.
Creating a PID Controller
To create an EasyPIDController, provide the proportional, integral, and derivative gains:
= new EasyPIDController(kP, kl, kD);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 how quickly the measurement changes. |
For example:
EasyPIDController pidController = new EasyPIDController(
0.5,
0.0,
0.02
);These gains are illustrative. The values your mechanism needs depend on its hardware, measurement units, and output units.
You can also create the controller using an existing EasyPIDCoefficients object:
= new EasyPIDController(coefficients);The controller copies the object's kP, kI, and kD values when it is created.
Example:
PIDCoefficients coefficients = new PIDCoefficients(
0.5,
0.0,
0.02
);
EasyPIDController pidController = new
EasyPIDController(coefficients);Setting the Target
Use setTargetPosition() to set the desired position:
controller.setTargetPosition(1000.O);For an encoder measured in ticks, this sets the target to 1000 ticks.
The target and current measurement must use the same units.
For example:
pidController.setTargetPosition(1000.0);To retrieve the current target:
= controller.getTargetPosition();The default target is 0.0.
Changing the target does not reset the controller's accumulated integral or timing history.
Now with both methods:
public void setDesired() {
pidController.setTargetPosition(1000.0);
}
public double getTarget() {
return pidController.getTargetPosition();
}Calculating the Output
Call calculate() once per control loop with the current measurement:
= controller.calculate(currentPosition);The controller calculates the error as:
targetPosition currentPosition;This means:
| Measurement | Error |
|---|---|
| Below the target | Positive |
| Equal to the target | Zero |
| Above the target | Negative |
For example:
controller.setTargetPosition(1000.0);
double outputThe error for this calculation is 250.0.
calculate() returns the controller output. It does not apply power to a motor automatically.
Providing the Target During Calculation
You can also provide the measurement and target in the same call:
= controller.calculate(currentPosition,
targetPosition);For example:
EasyMotor motor = new EasyMotor(hardwareMap, "motor");
double output = pidController.calculate(
motor.getCurrentPosition(),
1000.0
);This is equivalent to:
EasyMotor motor = new EasyMotor(hardwareMap, "motor");
pidController.setTargetPosition(1000.0);
double output = pidController.calculate(
motor.getCurrentPosition()
);The supplied target becomes the controller's stored target and remains active for future calculations.
How the Output Is Calculated
EasyPIDController combines three terms:
output = (kP * error)
+ (kI * integralSum)
+ (kD * derivative);Each term contributes differently:
| Term | Behavior |
|---|---|
| Proportional | Produces an output based on the current error. |
| Integral | Accumulates error over time to help correct persistent error. |
| Derivative | Responds to measurement changes to help damp motion. |
Automatic Timing
The controller uses System.nanoTime() to measure the elapsed time between calculations.
That elapsed time is converted to seconds and used for the integral and derivative calculations.
You do not need to provide a loop period:
= controller.calculate(currentPosition);On the first calculation after construction or reset(), the controller records the initial measurement and time. It does not accumulate integral error or calculate a measurement rate on that call.
Derivative on Measurement
The derivative term uses the negative rate of change of the measurement:
= (currentPosition lastMeasurement) / dt;This avoids a derivative spike caused solely by changing the target.
For example, changing the target from 500 to 1000 immediately changes the proportional error, but does not directly create a derivative spike.
Updating the PID Gains
Use setPID() to change all three gains:
pidController.setPID(0.05, 0.0, 0.2);You can also supply an existing EasyPIDCoefficients object:
PIDCoefficients coefficients = new PIDCoefficients(
0.5,
0.0,
0.02
);
pidController.setPID(coefficients);This copies the values from the object into the controller.
Updating the gains does not reset the accumulated integral or measurement history. To start with cleared controller state, call reset() separately:
pidController.setPID(0.05, 0.0, 0.2);
pidController.reset();Limiting the Integral
The integral term accumulates error over time:
integralSum += error * dt;If a mechanism cannot reach its target, this accumulated value can continue growing.
Use setMaxIntegralSum() to limit its magnitude:
controller.setMaxIntegralSum(200.O);This clamps the accumulated integral between:
-200.0 and 200.0The limit applies to integralSum, before multiplication by kI.
For example, with:
pidController.setPID(0.005, 0.001, 0.0002);
pidController.setMaxIntegralSum(200.0);the integral contribution is limited to a magnitude of:
9.001 * 200.0 0.2Negative limits are converted to positive values, so these calls are equivalent:
controller.setMaxIntegralSum(200.O);
controller.setMaxIntegralSum(-200.O);The default value of 0.0 means no integral limit is applied. It does not disable accumulation.
To remove a previously configured limit:
controller.setMaxIntegralSum(O.O);The clamp is applied during subsequent calculations with a positive elapsed time. It limits accumulation but does not automatically detect motor output saturation.
Setting a Tolerance
Use setTolerance() to define how close the measurement must be to the target:
controller.setTolerance(10.0);The tolerance uses the same units as the target and measurement.
For an encoder measured in ticks, this allows an error of up to 10 ticks in either direction.
For example, with a target of 1000 ticks, measurements from 990 through 1010 are within tolerance.
Negative tolerances are converted to positive values:
controller.setTolerance(-10.0);This has the same effect as:
controller.setTolerance(10.0);The default tolerance is 0.0, which requires the stored error to be exactly zero.
Checking Whether the Target Has Been Reached
Use atTarget() to check whether the most recently calculated error is within tolerance:
= controller.atTarget();The check is:
Math.abs(error) toleranceCall calculate() before checking atTarget():
double output = controller.calculate(
motor.getCurrentPosition()
);
if (controller.atTarget()) {
// The latest position measurement is within tolerance.
}atTarget() uses the stored error. It does not read the sensor or perform a new calculation.
Immediately after construction or reset(), the stored error is zero, so atTarget() returns true until a calculation updates that error. Changing the target also leaves the stored error unchanged until the next calculation.
Being within tolerance does not automatically stop the motor or make the controller output zero. The check also does not verify that the mechanism has stopped moving.
Reading the Error
Use getError() to retrieve the error from the latest calculation:
double error = controller.getError();For example:
double output = controller.calculate(
motor.getCurrentPosition()
);
telemetry.addData("Target", controller.getTargetPosition());
telemetry.addData("Error", controller.getError());
telemetry.addData("At Target", controller.atTarget());Like atTarget(), getError() uses the stored result from the last calculation.
Limiting the Output
EasyPIDController does not clamp its returned output.
If you use the output as FTC motor power, limit it to the motor power range before applying it:
double output = controller.calculate(
motor.getCurrentPosition()
);
double power = Math.max(-1.0, Math.min(1.0, output));
motor.setPower(power);To restrict the mechanism to a smaller power range:
double maxPower = 0.5;
double power = Math.max(
-maxPower,
Math.min(maxPower, output)
);
motor.setPower(power);This limits the applied motor power to -0.5 through 0.5.
The integral limit and output limit serve different purposes: setMaxIntegralSum() limits accumulated error, while the external clamp limits the final command sent to the motor.
Resetting the Controller
Use reset() to clear the controller's internal state:
controller.reset();This clears:
Accumulated integral.
Stored error.
Previous measurement.
Previous timestamp.
The next calculation is treated as the first update.
Resetting preserves the PID gains, target, tolerance, and integral limit.
For example:
controller.reset();
controller.setTargetPosition(1500.O);This prepares the controller to move toward a new target with cleared history.
Resetting is useful before resuming control after a pause because elapsed time is measured between calls to calculate().
Do not call reset() every loop. Doing so prevents the integral and derivative terms from developing normally.
reset() also does not stop the motor. To stop a mechanism, command its motor separately:
motor.setPower(0.0);
controller.reset();Chaining Configuration Methods
setPID(), setMaxIntegralSum(), and setTolerance() return the controller, allowing configuration calls to be chained:
EasyPIDController controller
new EasyPIDContr011er(0.005, 0.0, 0.0002)
. setMaxIntegralSum(200.0)Set the target separately:
controller.setTargetPosition(1000.O);setTargetPosition() and reset() return void and cannot be included in the chain.
Complete Example
The following FTC example controls a motor's encoder position using EasyPIDController.
Press A to select a target of 1000 ticks. Press B to return to the starting encoder position.
The encoder is reset during initialization, so position 0 represents the mechanism's position when the OpMode is initialized. This is not an automatic homing procedure.
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 dev.easyfamily.easystack.control.EasyPIDController;
@TeleOp(name = "EasyPID Position Example")
public class EasyPIDPositionExample extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
DcMotor motor = hardwareMap.get(
DcMotor.class,
"positionMotor"
);
motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// Illustrative gains: tune them for your mechanism.
EasyPIDController controller =
new EasyPIDController(0.005, 0.0, 0.0002)
.setMaxIntegralSum(200.0)
.setTolerance(10.0);
controller.setTargetPosition(0.0);
telemetry.addLine("A: 1000 ticks");
telemetry.addLine("B: 0 ticks");
telemetry.update();
waitForStart();
controller.reset();
try {
while (opModeIsActive()) { if (gamepad1.a) {
controller.setTargetPosition(1000.0);
} else if (gamepad1.b) {
controller.setTargetPosition(0.0);
}
double currentPosition = motor.getCurrentPosition();
double output =
controller.calculate(currentPosition);
double maxPower = 0.5;
double power = Math.max(
-maxPower,
Math.min(maxPower, output)
);
motor.setPower(power);
telemetry.addData(
"Target", controller.getTargetPosition()
telemetry.addData("Position", currentPosition);
telemetry.addData("Errors", controller.getError());
telemetry.addData("At Target",
controller.atTarget());
telemetry.addData("power", power);
telemetry.update();
idle();
} finally {
motor.setPower(O.O);RUN_WITHOUT_ENCODER allows this controller to calculate the motor power while the encoder remains available for position readings.
Configure the motor so that positive power increases the measured position. The controller assumes that a positive output moves the measurement toward a higher target.
This example continues running the controller when the mechanism enters the tolerance range, allowing it to keep correcting position error.
Última actualización 2026-11-16