EasyCascadeController
Cascade Controller
A cascade controller connects two controllers so that the output of the first becomes part of the target for the second.
For position control, the primary controller responds to position error and produces a velocity correction. The secondary controller uses this correction to calculate the final output based on the mechanism's velocity.
EasyStack's EasyCascadeController connects these two control loops and estimates velocity automatically from changes in the position measurement.
Creating a Cascade Controller
To create an EasyCascadeController, provide the primary and secondary controllers in the following order:
EasyCascadeController controller —
new EasyCascadeController(primary, secondary);The parameters are:
| Parameter | Description |
|---|---|
| primary | Position controller whose output is interpreted as a velocity correction. |
| secondary | Velocity controller that calculates the final mechanism output. |
Both controllers must be compatible with the EasyController base class used by this implementation.
Provide two separate, non-null controller instances. Each controller maintains its own control state.
How Cascade Control Works
During each calculation, EasyCascadeController performs three operations:
Estimates the measured velocity from changes in position.
Runs the primary controller to calculate a velocity correction.
Runs the secondary controller to calculate the final output.
The primary calculation is:
double velocityCorrection = primary.calculate(
currentPosition,
targetPosition
);The secondary calculation is:
double output = secondary.calculate(
measuredVelocity,
velocityCorrection + targetVelocity
);The secondary controller therefore receives the following target:
secondaryTarget = velocityCorrection + targetVelocity;For example, if the primary controller requests a correction of 80 ticks per second and the configured target velocity is 200 ticks per second, the secondary controller receives a target of 280 ticks per second.
The primary controller's output is not sent directly to the motor.
Setting a Position Target
Use setSetPoint() to configure a position target:
controller.setSetPoint(1000.0);For position measurements in encoder ticks, this sets the target to 1000 ticks.
This method also sets the target velocity to zero:
controller.setSetPoint(1000.0);This is equivalent to:
controller.setSetPoints(1000.0, 0.0);A zero target velocity does not prevent movement. The primary controller still generates a velocity correction whenever the position differs from the target.
As the mechanism approaches its target, the required position correction can decrease.
Setting Position and Velocity Targets
Use setSetPoints() to provide both targets:
controller.setSetPoints(targetPosition, targetVelocity);The parameters are:
| Parameter | Description |
|---|---|
| psp | Target position. |
| vsp | Target velocity added to the primary controller's output. |
For example:
controller.setSetPoints (1000.O, 200.0);This configures:
| Target | Value |
|---|---|
| Position | 1000 position units |
| Velocity | 200 position units per second |
The velocity target is added to the correction produced by the primary controller. It is not a maximum velocity limit.
Following a Moving Target
Position and velocity targets can be updated together each loop:
controller.setSetPoints(
referencePosition,
referenceVelocity
);
double output = controller.calculate(currentPosition);This is useful when another part of your program supplies a moving reference, such as a motion profile.
EasyCascadeController does not generate a motion profile internally. Your program must provide the reference position and velocity.
For a stationary final target, use a velocity target of zero:
controller.setSetPoints(finalPosition, 0.0);A nonzero velocity target continues contributing to the secondary target even when the position error is zero.
Calculating the Output
Using the inherited public calculate() method, provide the current position:
= controller.calculate(currentPosition);For example:
controller.setSetPoint(.O);
double currentPosition motor.getCurrentPosition();
= controller.calculate(currentPosition);The subclass implements its cascade logic inside calculateOutput(). That method is protected and is intended to be called through the base controller's public calculation interface.
Call the controller once per control loop with a fresh position measurement.
The returned value is the secondary controller's output. The cascade class does not apply it to a motor automatically.
Automatic Velocity Measurement
EasyCascadeController estimates velocity using the change in position divided by elapsed time:
measuredVelocity =
(currentPosition - previousPosition) / period;The elapsed time is measured in seconds using System.nanoTime().
The resulting velocity units depend on the position units:
| Position measurement | Estimated velocity |
|---|---|
| Encoder ticks | Ticks per second |
| Rotations | Rotations per second |
| Degrees | Degrees per second |
| Radians | Radians per second |
| Meters | Meters per second |
The position target must match the position measurement's units. The velocity target and primary controller's output must match the estimated velocity's units.
First Calculation
On the first calculation, the controller records the initial position and timestamp.
There is no previous measurement from which to estimate velocity, so a newly created controller uses its initial velocity value of 0.0.
Later calculations update the velocity when the measured time interval is greater than 1E-6 seconds.
If the interval is too small, the previous velocity estimate is retained, although both child controllers are still evaluated.
Measurement Quality
The velocity estimate is calculated directly from position differences. This class does not apply a velocity filter.
Noisy position readings or coarse encoder resolution can therefore produce a noisy velocity estimate, especially over short time intervals.
Use a consistent control loop and tune the secondary controller with the quality of this estimate in mind.
Reading the Measured Velocity
Use getMeasuredVel() to retrieve the latest velocity estimate:
= controller.getMeasuredVel();For example:
double output = controller.calculate(
motor.getCurrentPosition()
);
telemetry.addData(
"Measured Velocity",
controller.getMeasuredVel()
);getMeasuredVel() does not read the sensor or perform a new calculation. It returns the estimate stored during the latest controller update.
Position and Velocity Errors
The cascade controller updates its inherited position error using:
positionError = targetPosition measuredPosition;Its inherited velocity error is updated using:
velocityError = targetVelocity measuredVelocity;However, the secondary controller receives the combined target:
velocityCorrection + targetVelocityThis means the cascade controller's stored velocity error is different from the error used by the secondary controller whenever the primary output is nonzero.
For example:
| Value | Amount |
|---|---|
| Target velocity | 200 ticks/s |
| Primary correction | 80 ticks/s |
| Measured velocity | 150 ticks/s |
| Cascade's stored velocity error | 50 ticks/s |
| Secondary controller's target minus measurement | 130 ticks/s |
This distinction matters when interpreting error values or completion checks provided by the base class.
Limiting the Output
EasyCascadeController returns the secondary controller's result without adding its own output clamp.
If you use that result as FTC motor power, limit it before applying it:
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:
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 cascade class also does not limit the velocity correction produced by the primary controller. Any required correction limit must be handled by the primary controller or an additional implementation.
Resetting the Controller
Use reset() to reset the cascade:
controller.reset();This method:
Calls the base controller's reset().
Sets the estimated velocity to zero.
Resets the primary controller.
Resets the secondary controller.
The exact behavior of the inherited state and child controllers depends on their respective reset() implementations.
For a clear restart sequence, reset first and then set the desired targets:
controller.reset();
controller.setSetPoints(1000.0, 0.0);The velocity estimator's first-sample behavior depends on the base reset clearing lastTimeStamp to zero. With that timestamp cleared, the next calculation records a new position baseline.
Do not reset the controller every loop, because doing so repeatedly clears the child controllers' history.
Resetting does not directly stop the motor. Command zero power separately:
motor.setPower(0.0);
controller.reset();Configuring the Two Controllers
Configure each child controller through its own methods before passing it to the cascade.
Their roles require different tuning:
| Controller | Input measurement | Output meaning |
|---|---|---|
| Primary | Position | Velocity correction |
| Secondary | Velocity | Final actuator command |
The cascade stores references to the supplied controllers. It does not copy them.
If you retain those references, later changes to their configuration also affect the cascade.
Because each child maintains its own state, avoid running additional independent calculations on those same instances while the cascade is using them.
Complete Usage Example
The following helper shows position control using two compatible, already configured EasyController instances.
The motor should already be configured for direct power control, and its encoder position should use the same reference as the target.
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.hardware.DcMotor;
import dev.easyfamily.easystack.control.EasyCascadeController;
import dev.easyfamily.easystack.control.EasyController;
public class CascadePositionControl {
private final DcMotor motor;
private final EasyCascadeController controller;
public CascadePositionControl(
DcMotor motor,
EasyController positionController,
EasyController velocityController
) {
this.motor = motor;
controller = new EasyCascadeController( positionController,
velocityController
);
}
public void start(double targetPosition) {
controller.reset();
controller.setSetPoint(targetPosition);
}
public void setTargetPosition(double targetPosition) {
controller.setSetPoint(targetPosition);
}
public void setReference(
double targetPosition,
double targetVelocity
) {
controller.setSetPoints(
targetPosition, targetVelocity
);
}
public void update() {
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);
}
public double getMeasuredVelocity() {
return controller.getMeasuredVel(); }
public void stop() {
motor.setPower(0.0);
controller.reset();
}
}Create the helper using your configured child controllers:
CascadePositionControl mechanism =
new CascadePositionControl(
motor,
positionController,
velocityController
);
mechanism.start(1000.0);Inside the active OpMode loop:
mechanism.update();
telemetry.addData(
"Measured Velocity",
mechanism.getMeasuredVelocity()
);
telemetry.update();To follow a moving position and velocity reference, update both before calculating:
mechanism.setReference(
referencePosition,
referenceVelocity
);
mechanism.update();When control ends:
mechanism.stop();Controller Compatibility
This implementation extends an EasyController base class and uses inherited members such as setPoint, lastTimeStamp, and calculateOutput().
The previously shown EasyPIDController instead implements dev.easyfamily.EasyStack.controllable.EasyController and uses methods such as setTargetPosition().
As provided, these snippets use different controller contracts. The earlier EasyPIDController cannot be assumed to work as a child of this cascade without adapting it to the base class used here.
The usage examples above assume that this base class exposes a public calculate(double) method that invokes calculateOutput(double). Its source is needed to verify additional inherited configuration and completion methods.
Última atualização 2026-11-16