EasyStack Docs

EasyPIDFController

Esta página ainda não foi traduzida. Mostrando a versão em inglês.

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:

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

Java
EasyPIDFController controller —
        new EasyPIDFController(kP, kl, kD, kF);

The parameters are:

ParameterDescription
kPProportional gain. Responds to the current error.
kIIntegral gain. Responds to accumulated error over time.
kDDerivative gain. Responds to changes in the measurement.
kFFeedforward gain. Multiplies the target to produce an additional output.

For example:

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

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

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

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

Java
 = controller.getTargetPosition();

The default target is 0.0.

Calculating the Output

Use calculate() with the current measurement:

Java
double output = controller.calculate(currentMeasurement);

The controller first calculates the inherited PID output:

Java
double pidOutput super.calculate(currentMeasurement);

It then adds feedforward:

Java
double feedforward kF * getTargetPosition();

The final output is:

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

Java
double output = controller.calculate(
        currentMeasurement,
        target
);

For example:

Java
double output = controller.calculate(
        motor.getVelocity(),
        3000.0
);

This updates the stored target and calls the PIDF calculation, including feedforward.

It is equivalent to:

Java
controller.setTargetPosition(3000.0);

double output = controller.calculate(
        motor.getVelocity()
);

How Feedforward Works

Feedforward is calculated directly from the target:

Java
feedforward kF * target;

For example, with:

Java
controller.setF(0.00015);
controller.setTargetPosition(3000.O);

the feedforward contribution is:

Java
0.00015 * 0.45

If the PID portion produces 0.10, the combined output is:

Java
0.10 + 0.45 0.55

For a positive kF, feedforward follows the sign of the target:

TargetFeedforward with kF = 0.00015
30000.45
10000.15
00.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:

Java
kF = motorPower / targetVelocity;

For example, if testing shows that a steady power of 0.45 maintains approximately 3000 ticks per second:

Java
     0.45 / 300.0;

This gives an initial estimate of:

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

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

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

Java
controller.setF(O. 00018);

The PID gains remain unchanged.

To disable feedforward:

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

Java
controller.setPIDF(
        0.00025,
        0.0,
        0.0,
        0.00016
);

You can also supply an existing EasyPIDFCoefficients object:

Java
controller.setPIDF(coefficients);

This copies the coefficient values into the controller.

To update only the PID gains, use the inherited setPID() method:

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

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

Java
controller.setMaxIntegralSum(200.O);

This limits the accumulated integral to the range:

Java
-200.0 to 200.0

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

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

Java
double output = controller.calculate(
        motor.getVelocity()
);

boolean atSpeed = controller.atTarget();

The inherited check is:

Java
Math.abs(error) tolerance

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

Java
double error = controller.getError();

For example:

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

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

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

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

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

Java
motor.setPower(0.0);
controller.reset();

Chaining Configuration Methods

setF() and setPIDF() return an EasyPIDFController, so they support chaining:

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

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

Java
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);
Java
        // 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();
Java
        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,
Java
                            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)",
Java
                            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.

Última atualização 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.