EasyController
Base Controller
EasyController provides a common foundation for EasyStack controllers.
It handles setpoints, error tolerances, output limits, and an optional output compensation term. Subclasses implement the control algorithm used to calculate the initial output.
This allows different controllers to share the same methods for setting targets, calculating outputs, and checking whether a target has been reached.
Creating a Controller
EasyController is an abstract class, so it cannot be instantiated directly.
Instead, use a compatible subclass or create your own:
public class ProportionalController extends EasyController {
private final double kP;
public ProportionalController(double kP) {
this.kP = kP;
}
@Override
protected double calculateOutput(double pv) {
measuredValue = pv;
errorVal_p = setPoint - measuredValue;
errorVal_v = 0.0;
return kP * errorVal_p;
}
}This minimal example calculates a proportional output from the position error. It sets the velocity error to zero because it does not estimate the error's rate of change.
Create an instance using:
ProportionalController controller —
new;The gain is illustrative and must be adjusted for your mechanism.
Setting the Setpoint
Use setSetPoint() to set the desired target:
controller.setSetPoint(1000.0);For a controller receiving encoder ticks, this represents a target of 1000 ticks.
The setpoint and measurement must use the same units.
To retrieve the setpoint:
= controller.getSetPoint();The default setpoint is 0.0.
Setting a new setpoint also updates the stored errors using the existing measurement and timing state. It does not read a sensor or reset the controller.
Calculating the Output
Use calculate() with the current measurement:
= controller.calculate(currentPosition);For example:
controller.setSetPoint(1000.0);
double output = controller.calculate(
motor.getCurrentPosition()
);The parameter is:
| Parameter | Description |
|---|---|
| pv | Current process value, such as measured position. |
The base controller processes the result in this order:
Calls the subclass's calculateOutput() method.
Adds the configured openF compensation.
Checks whether the stored errors are within tolerance.
Applies output magnitude limits if the controller is outside tolerance.
The returned value is not automatically applied to a motor.
Providing the Setpoint During Calculation
You can provide the measurement and setpoint together:
double output = controller.calculate(
currentPosition,
targetPosition
);The parameters are:
| Parameter | Description |
|---|---|
| pv | Current measurement. |
| sp | New setpoint. |
For example:
double output = controller.calculate(
motor.getCurrentPosition(),
1000.0
);This calls setSetPoint() before calculating the output. The supplied setpoint remains stored for later calculations.
Using the Stored Measurement
A no-argument overload is also available:
= controller.calculate();This recalculates the output using measuredValue, the measurement stored by the subclass.
It does not obtain a fresh sensor reading.
For normal feedback control, supply an updated measurement each loop:
double output = controller.calculate(
motor.getCurrentPosition()
);Position Error
The base class calculates position error when a setpoint is assigned:
errorValV p = setPoint measu;For example, with a target of 1000 and a measurement of 750, the error is 250.
Use getPositionError() to read the stored value:
double error = controller.getPositionError();During calculations, the subclass is responsible for keeping the measurement and error fields up to date.
A common telemetry pattern is:
double output = controller.calculate(
motor.getCurrentPosition()
);
telemetry.addData("Target", controller.getSetPoint());
telemetry.addData("Position Error", controller.getPositionError());Velocity Error
Use getVelocityError() to retrieve the stored velocity error:
double velocityError = controller.getVelocityError();The base implementation of setSetPoint() calculates it as the rate of change of position error:
errorValV v = (errorVal_p prevErrorVal) / period;For position measured in ticks and a period measured in seconds, this value has units of ticks per second.
Subclasses may update this field differently. For example, a cascade controller can use the difference between a requested velocity and a measured velocity.
Check the concrete controller's behavior when interpreting this value.
Before the First Calculation
The initial period is 0.0. Calling the base setSetPoint() before a valid period is available can therefore produce Infinity or NaN for the velocity error.
A subclass should initialize or update its timing and error state during calculation before those values are used for completion checks.
Setting a Position Tolerance
Use setTolerance() to define the allowed position error:
controller.setTolerance(10.0);For encoder ticks, this requires the absolute position error to be less than 10 ticks.
The single-argument overload also sets the velocity tolerance to positive infinity:
controller.setTolerance(10.0);This is equivalent to:
controller.setTolerance(
10.0,
Double.POSITIVE_INFINITY
);A finite velocity error therefore passes the velocity check.
Setting Position and Velocity Tolerances
To require both errors to be small, provide two tolerances:
controller.setTolerance(10.0, 25.0);The parameters are:
| Parameter | Description |
|---|---|
| positionTolerance | Allowed magnitude of position error. |
| velocityTolerance | Allowed magnitude of the stored velocity error. |
With this configuration, the controller requires:
Math.abs(positionError) < 10.0and:
Math.abs (velocityError) < 25.0The defaults are:
| Setting | Default |
|---|---|
| Position tolerance | 0.05 |
| Velocity tolerance | Double.POSITIVE_INFINITY |
Tolerances are stored exactly as provided. Negative values are not converted to positive values.
Use positive tolerances. Because the comparisons are strict, a tolerance of zero cannot be satisfied, even when the corresponding error is zero.
Checking Whether the Setpoint Has Been Reached
Use atSetPoint():
= controller.atSetPoint();It returns true only when both stored errors are strictly within their respective tolerances:
Math.abs(errorValV p) < errorTolerance p
&& Math.abs(errorValV v) < errorTolerance vFor example:
double output = controller.calculate(
motor.getCurrentPosition()
);
if (controller.atSetPoint()) {
// Both stored errors are within tolerance.
}An error exactly equal to its tolerance does not pass.
atSetPoint() does not read a sensor or perform a new calculation. Its result depends on the stored errors, so check it after calculating with a current measurement.
With the base class's initial zero errors and default tolerances, it can return true before any measurement has been processed. NaN or infinite errors do not pass the corresponding comparison, even with an infinite tolerance.
Reaching the setpoint does not automatically stop the motor or force the output to zero.
Reading the Tolerances
Use getTolerance() to retrieve both values:
double[] tolerances — controller.getTolerance();
double positionTolerance tolerances[O];
double velocityTolerance tolerances[l];The returned order is:
| Index | Value |
|---|---|
| 0 | Position tolerance. |
| 1 | Velocity tolerance. |
The method returns a new array. Modifying that array does not change the controller's configuration.
Minimum Output
Use setMinOutput() to configure the minimum output magnitude applied outside tolerance:
controller.setMinOutput(0.1);For a nonzero calculated output, this can help avoid commands too small to move a mechanism.
For example, outside tolerance:
| Output before limiting | Output after a minimum of 0.1 |
|---|---|
| 0.03 | 0.1 |
| -0.03 | -0.1 |
| 0.4 | 0.4 |
| 0.0 | 0.0 |
An exactly zero output remains zero because Math.signum(0.0) is zero.
Negative minimum values are converted to positive values:
controller.setMinOutput(-0.1);This is equivalent to:
controller.setMinOutput(0.1);To read the configured minimum:
double minimum = controller.getMinOutput();The default minimum is 0.0.
Maximum Output
Use setMaxOutput() to configure the maximum output magnitude applied outside tolerance:
controller.setMaxOutput(0.75);For example, outside tolerance:
| Output before limiting | Output after a maximum of 0.75 |
|---|---|
| 1.2 | 0.75 |
| -1.2 | -0.75 |
| 0.4 | 0.4 |
To read the configured maximum:
double maximum = controller.getMaxOutput ();The default maximum is Double.POSITIVE_INFINITY, which provides no finite upper bound.
Unlike setMinOutput(), this method does not take the absolute value. Configure a nonnegative maximum that is greater than or equal to the minimum:
controller.setMinOutput(O. 1);
controller.setMaxOutput(@.75)Output Limits Within Tolerance
The configured minimum and maximum are bypassed when atSetPoint() returns true .
In that case, calculate() returns the subclass output plus openF compensation directly.
If your actuator requires an output bound on every calculation, apply a final clamp before sending the command:
double output = controller.calculate(
motor.getCurrentPosition()
);
double power = Math.max(-0.75, Math.min(0.75, output));
motor.setPower(power);This maintains the chosen motor power range whether or not the controller is within tolerance.
Open-Loop Compensation
Use setOpenF() to add a fixed compensation term in the direction of the position error:
controller.setOpenF(0.05);The added term is:
Math.signum(errorValV p) * openFWith openF set to 0.05:
| Position error | Added compensation |
|---|---|
| Positive | +0.05 |
| Negative | -0.05 |
| Zero | 0.0 |
For example, if the subclass returns 0.2 and the position error is positive:
output 0.2 + 0.05;The resulting output before limiting is 0.25.
This compensation can help overcome resistance near small errors. It follows the sign of the position error, not necessarily the sign of the subclass's output or the direction of motion.
It is added before output limiting and remains active within tolerance whenever the position error is nonzero.
The default value is 0.0.
To disable it:
controller.setOpenF(O.O);Negative values are accepted as provided and reverse the compensation's direction.
Reading the Control Period
Use getPeriod() to retrieve the stored calculation period:
= controller.getPeriod();The base class does not measure elapsed time automatically. The subclass must update period.
For subclasses that store elapsed time in seconds:
telemetry.addData(
"Period (ms)",
controller.getPeriod() * 1000.0
);The period starts at 0.0. Its later value and timing behavior depend on the concrete controller.
Resetting the Controller
Use reset():
controller.reset();The base implementation clears only:
prevErrorVal.
lastTimeStamp.
It does not clear the setpoint, measurement, current errors, period, tolerances, output limits, or openF.
Subclasses may override reset() to clear additional state, such as an integral accumulator or velocity estimate.
Resetting does not command a motor to stop:
motor.setPower(0.0);
controller.reset();Reset Behavior in Subclasses
The base constructor calls reset(). Java can therefore invoke a subclass's override before the subclass constructor has initialized its fields.
An overridden reset method should account for that initialization order, particularly when resetting child objects.
Chaining Configuration Methods
The output configuration methods return the controller, allowing them to be chained:
controller
. setMinOutput(0. 1)
. setMaxOutput(0. 75)
. setOpenF(0.05);Set tolerance and setpoint separately:
controller.setTolerance(10.0, 25.0);
controller.setSetPoint(1000.0);setTolerance(), setSetPoint(), and reset() return void and cannot be chained.
Creating a Custom Controller
A subclass implements:
@Override
protected double calculateOutput(double pv) {
// Update measurement, error, and timing state.
// Return the raw controller output.
}The base calculate() method handles compensation and conditional output limiting afterward.
The following proportional controller also tracks the rate of change of error for tolerance checks:
package org.firstinspires.ftc.teamcode;
import dev.easyfamily.easystack.control.EasyController;
public class ProportionalController extends EasyController {
private final double kP;
public ProportionalController(double kP) {
this.kP = kP;
}
@Override
protected double calculateOutput(double pv) {
double now = System.nanoTime() / 1e9;
boolean firstUpdate = lastTimeStamp == 0.0;
period = firstUpdate ? 0.0 : now - lastTimeStamp;
measuredValue = pv; errorVal_p = setPoint - measuredValue;
if (!firstUpdate && period > 0.0) {
errorVal_v = (errorVal_p - prevErrorVal) / period;
} else {
errorVal_v = 0.0;
}
prevErrorVal = errorVal_p;
lastTimeStamp = now;
return kP * errorVal_p;
}
}This controller uses only proportional error to calculate its raw output. The error rate is recorded for telemetry and atSetPoint() checks.
Complete Usage Example
Using the custom controller above, create and configure an instance:
ProportionalController controller —
new;
controller
. setMinOutput(0. 05)
. setMaxOutput(0.5)
. setOpenF(0.02);
controller.setTolerance(10.0, 25.0);The values are illustrative. Tune them for your mechanism and measurement units.
For FTC motor power control, configure the motor to accept direct power commands:
motor.setM0de(DcM0tor.RunM0de.RUN WITHOUT ENCODER);This mode still allows encoder position readings. The target must use the same encoder reference as the measurement.
Inside the active OpMode loop:
double currentPosition = motor.getCurrentPosition();
double output = controller.calculate(currentPosition);
// Enforce the power limit even when within tolerance.
double power = Math.max(-0.5, Math.min(0.5, output));
motor.setPower(power);
telemetry.addData("Target", controller.getSetPoint());
telemetry.addData("Position", currentPosition);
telemetry.addData("Position Error", controller.getPositionError());
telemetry.addData("Velocity Error", controller.getVelocityError());
telemetry.addData("At Setpoint", controller.atSetPoint());
telemetry.addData("Power", power);
telemetry.update();Configure the mechanism so that positive motor power increases the measured position.
When control ends, stop the motor:
motor.setPower(0.0);
controller.reset();Última actualización 2026-11-16