EasyStack Docs

EasySquIDFController

Esta página aún no está traducida. Se muestra la versión en inglés.

SquIDF Controller

A SquIDF controller combines a square-root proportional response with integral, derivative, and feedforward terms.

Instead of multiplying the position error directly by kP, it uses the square root of the error's magnitude while preserving its sign.

EasyStack's EasySquIDFController provides this nonlinear response along with configurable integral limits, integral decay, and optional integral clearing near the target.

Creating a SquIDF Controller

To create an EasySquIDFController, provide four gains:

Java
EasySquIDFController controller =
        new EasySquIDFController(kP, kI, kD, kF);

The parameters are:

ParameterDescription
kPGain applied to the signed square root of the error.
kIGain applied to accumulated error over time.
kDGain applied to the rate of change of error.
kFFeedforward gain multiplied by the target.

For example:

Java
EasySquIDFController controller =
        new EasySquIDFController(
                0.02,
                0.0,
                0.0,
                0.0
        );

These values are illustrative. Tune the gains for your mechanism and measurement units.

Square-Root Proportional Control

A conventional proportional controller calculates:

Java
proportional kp * error;

EasySquIDFController instead calculates:

Java
double signedRoot
        Math.signum(error) * Math.sqrt(Math.abs(error));
double proportional kP * signedRoot;

The sign is preserved so the controller can correct errors in either direction.

For example:

ErrorSigned square root
10010
255
11
00
-25-5
-100-10

The response grows more slowly than a linear proportional response as error magnitude increases.

For the same numerical gain and units, the square-root response is smaller than the linear response when the error magnitude exceeds 1, and larger when it is between 0 and 1.

This changes the response shape; it does not automatically guarantee smoother movement or eliminate overshoot. The square-root response can also be sensitive to small measurement changes near zero.

Because the proportional formula is different, conventional PID gains should not be reused without retuning.

Setting the Target

Use setTargetPosition():

Java
controller.setTargetPosition(1000.O);

For encoder measurements in ticks, this sets a target of 1000 ticks.

The target and measurement must use the same units.

To retrieve the target:

Java
 = controller.getTargetPosition();

The default target is 0.0.

Changing the target does not reset the accumulated integral, previous error, or timer.

Calculating the Output

Call calculate() with the current measurement:

Java
 = controller.calculate(currentPosition);

For example:

Java
controller.setTargetPosition(1000.0);

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

The position error is:

Java
        targetPosition currentPosition;

The final output combines four terms:

Java
output = (kP * signedRoot)
      + (kI * totalError)
      + (kD * errorRate)
      + (kF * targetPosition);

calculate() returns this output. It does not apply motor power automatically.

Providing the Target During Calculation

You can supply both the measurement and target:

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

For example:

Java
double output = controller.calculate(
        motor.getCurrentPosition(),
        1000.0
);

This is equivalent to:

Java
controller.setTargetPosition(1000.0);

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

The target remains stored for subsequent calculations.

Automatic Timing

The controller uses FTC's ElapsedTime to measure the time between calculations in seconds.

You do not need to supply a loop period.

Each calculation reads the elapsed time and resets the timer:

Java
double period timer.seconds();
timer.reset();

If the measured period is exactly zero, the implementation substitutes 1e-6 seconds.

The derivative is calculated only when the period is greater than 1e-6 seconds. Integral accumulation still occurs using the selected period.

First Calculation

The first calculation uses the elapsed time since construction or the most recent reset().

Unlike the earlier EasyPIDController, this implementation does not skip integral accumulation or explicitly suppress the derivative on its first update.

The previous error starts at zero, so the first derivative can be large if the initial error is nonzero and the elapsed time is short.

Reset the controller when beginning a new control session to avoid including a long idle period in the integral. If kD is enabled, also account for the first-update derivative behavior.

Derivative on Error

The derivative term uses the rate of change of error:

Java
           = (error previousError) / period;

This responds to changes in both the measurement and the target.

An abrupt target change can therefore create a derivative spike, even if the mechanism has not moved.

For mechanisms where this is undesirable, use a gradually changing target or consider a controller that calculates its derivative from the measurement.

Limiting the Integral

The integral accumulates error over time:

Java
totalError += period * error;

Use setIntegrationBounds() to limit the accumulated value:

Java
controller.setIntegrationBounds(200.0);

This clamps totalError between:

Java
-200.0 and 200.0

The limit applies before multiplication by kI.

For example, with:

Java
controller.setPIDF(0.02, 0.001, 0.0, 0.0);
controller.setIntegrationBounds(200.0);

the integral contribution has a maximum magnitude of 0.2, provided the subsequent decay factor does not amplify the accumulator:

Java
9.001 * 200.0 0.2

Negative bounds are converted to positive values:

Java
controller.setIntegrationBounds(-200.0);

This has the same effect as:

Java
controller.setIntegrationBounds(200.0);

The default bound is 0.0, which means no integral clamp is applied. It does not disable accumulation.

Integral Decay

Use setDecayFactor() to reduce accumulated integral when its sign differs from the current error:

Java
controller.setDecayFactor(0.9);

The controller applies:

Java
if (Math.signum(totalError) != Math.signum(error)) {
    totalError *= decayFactor;
}

For example, if the accumulated integral is positive but the mechanism moves past the target, the current error becomes negative. Decay can reduce the remaining positive integral.

Common settings are:

FactorBehavior when signs differ
1.0Leaves the accumulator unchanged.
0.9Retains 90% of the accumulator per qualifying calculation.
0.5Retains 50% per qualifying calculation.
0.0Clears the accumulator.

The default is 1.0, so no decay occurs by default.

Decay is applied per calculation, not per second. Its effective strength therefore depends on how frequently the controller runs.

The sign comparison also applies when the current error is exactly zero and the accumulator is nonzero.

Choosing a Decay Factor

Use values from 0.0 to 1.0 for normal decay behavior.

The method does not validate or clamp the factor:

Values above 1.0 amplify the accumulator.

Negative values reverse its sign.

Decay runs after the integral clamp. A factor whose magnitude exceeds 1.0 can therefore increase the accumulator beyond the configured bound.

Clearing the Integral at the Setpoint

Use setClearAtSetpoint() to clear accumulated integral when the measurement is within tolerance:

Java
controller.setClearAtSetpoint(true, 10.0);

The parameters are:

ParameterDescription
clearWhether to clear the integral while within tolerance.
toleranceAllowed absolute difference between target and measurement.

With this configuration:

Java
Math.abs (targetPosition currentPosition) <= 10.0

causes:

Java
totalError

Clearing occurs after accumulation, clamping, and decay, before the final output is calculated.

It clears only the integral. The square-root proportional, derivative, and feedforward terms remain active.

Clearing the integral may be unsuitable if the mechanism relies on integral output to hold its target.

Disabling Automatic Clearing

To disable clearing while keeping a chosen tolerance:

Java
controller.setClearAtSetpoint(false, 10.0);

The defaults are:

SettingDefault
Clear integral at setpointfalse
Position tolerance0.05

The tolerance is stored exactly as provided. Use a nonnegative value; negative tolerances cannot pass the setpoint check.

Checking Whether the Target Has Been Reached

Use atSetpoint() with the current measurement:

Java
boolean reachedTarget
        controller.atSetpoint(currentPosition);

For example:

Java
double currentPosition motor.getCurrentPosition();
boolean reachedTarget
        controller.atSetpoint(currentPosition);

The check is:

Java
Math.abs (targetPosition currentPosition)
        <= positionTolerance;

An error exactly equal to the tolerance passes.

Unlike a check based on stored error, this method evaluates the measurement you supply directly. It does not require a preceding calculate() call.

It also does not update the controller's integral, derivative history, or timer.

Being within tolerance does not verify that the mechanism has stopped moving and does not automatically stop the motor.

Feedforward

Feedforward is proportional to the target:

Java
feedforward kF * targetPosition;

For example, with:

Java
controller.setPIDF(0.02, 0.0, 0.0, o. 0001);
controller.setTargetPosition(. 0)

the feedforward contribution is:

Java
0.0001 * 1000.0 0.1

This term remains 0.1 even when the position error reaches zero.

It is not automatically a gravity compensation or static friction model. For position control, determine whether a target-proportional output is appropriate for the mechanism.

To disable feedforward, set kF to zero:

Java
controller.setPIDF(0.02, 0.0, 0.0, 0.0);

Updating the Gains

Use setPIDF() to update all four gains:

Java
controller.setPIDF(
        0.025,
        0.001,
        0.0001,
        0.0
);

Changing the gains preserves the target, accumulated integral, previous error, and timing state.

To clear the control history as well:

Java
controller.setPIDF(0.025, 0.001, 0.0001, 0.0);
controller.reset();

This implementation provides a four-number constructor and a four-number setPIDF() method. It does not define a coefficients-object overload or a separate setF() method.

Limiting the Output

EasySquIDFController does not limit the final output.

When using it as FTC motor power, clamp the result before applying it:

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

double power = Math.max(-1.0, Math.min(1.0, output));

motor.setPower(power);

To use a smaller power range:

Java
double maxPower = 0.5;

double power = Math.max(
        -maxPower,
        Math.min(maxPower, output)
);

motor.setPower(power);

The integral bound limits only the accumulator. It does not limit the combined output or automatically account for motor saturation.

Resetting the Controller

Use reset():

Java
controller.reset();

This clears:

Previous error.

Accumulated integral.

Elapsed timer.

It preserves the gains, target, integration bound, decay factor, clearing setting, and tolerance.

For example:

Java
controller.reset();
controller.setTargetPosition(1000.O);

Do not reset every loop. Repeated resets prevent normal integral and derivative history from developing.

Resetting does not stop the motor:

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

Chaining Configuration Methods

The configuration methods return the controller, allowing them to be chained:

Java
EasySquIDFController controller —
        new EasySqu1DFContr011er(0.02, 0.0, 0.0, 0.0)
                 . setIntegrationBounds(200.0)
                 . setDecayFactor(0.9)
                 . setClearAtSetpoint(true, 10.0);

Set the target separately:

Java
controller.setTargetPosition(1000.O);

setTargetPosition() and reset() return void and cannot be chained.

Complete Example

The following FTC example controls encoder position using EasySquIDFController.

Press A to select 1000 ticks. Press B to return to 0 ticks.

The encoder is reset during initialization, so zero represents the mechanism's starting position. This is not an automatic homing procedure.

The example starts with integral, derivative, and feedforward disabled so the square-root proportional response can be tuned first.

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 dev.easyfamily.easystack.control.EasySquIDFController;

@TeleOp(name = "EasySquIDF Position Example")
public class EasySquIDFPositionExample 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);
Java
        motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        // Illustrative gains: tune for your mechanism.
        EasySquIDFController controller =
                new EasySquIDFController(
                        0.02,
                        0.0,
                        0.0,
                        0.0
                )
                        .setIntegrationBounds(200.0)
                        .setDecayFactor(0.9)
                        .setClearAtSetpoint(true, 10.0);

        controller.setTargetPosition(0.0);

        telemetry.addLine("A: 1000 ticks");
        telemetry.addLine("B: 0 ticks");
        telemetry.update();
Java
        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(
Java
                        -maxPower,
                        Math.min(maxPower, output)
                );

                motor.setPower(power);

                double error =
                        controller.getTargetPosition()
                                - currentPosition;

                telemetry.addData(
                        "Target",
                        controller.getTargetPosition()
                );
                telemetry.addData("Position", currentPosition);
                telemetry.addData("Error", error);
                telemetry.addData(
                        "At Setpoint",
                        controller.atSetpoint(currentPosition)
                );
Java
                 telemetry.addData("power", power);
                 telemetry.update();
                 idle();
         } finally {
             motor.setPower(0.0);

RUN_WITHOUT_ENCODER allows the controller to calculate motor power while encoder readings remain available.

Configure the mechanism so that positive power increases the measured position. The controller continues calculating within tolerance; automatic integral clearing does not disable the other output terms.

Última actualización 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.