Advanced Subsystem Control
Learn how to use tolerances to check if a mechanism has reached its target, and how to store state across OpModes.
By the end you can
- Understand how to use mathematical tolerances for mechanisms.
- Learn how Commands can use subsystem boolean checks for the isFinished() method.
- Learn how to persist data across OpModes using static variables.
Once you start using PID Controllers to move your mechanisms to exact targets, you run into a new problem: How do you know when it's done?
In the physical world, a motor will never perfectly reach exactly 500.00000 encoder ticks. It might settle at 499 or 501, or even vibrate between them. If a Command waits for the motor to be exactly at 500, the Command will literally never finish!
Our team solves this by using Tolerances inside our subsystems, and checking those tolerances in our Commands.
Using Tolerances
Let's look at a real example from our LaunchSubsystem. The launch subsystem has to get the flywheel up to speed, aim the turret, and aim the visor before it allows the robot to shoot.
Instead of checking for exact numbers, we check if the current position is close enough to the target.
package org.firstinspires.ftc.teamcode.subsystems;
import com.arcrobotics.ftclib.command.SubsystemBase;
public class LaunchSubsystem extends SubsystemBase {
// We define how much error is acceptable for each mechanism
private static final double TARGET_RPM_TOLERANCE = 50.0; // RPM
private static final double TURRET_POSITION_TOLERANCE = 0.05; // Servo position
private static final double VISOR_POSITION_TOLERANCE = 0.05; // Servo position
private double targetRPM = 0;
private double targetTurretPos = 0;
private double targetVisorPos = 0;
// ... motor initialization code ...
/**
* Checks if the flywheel, turret, and hood are all within acceptable tolerances
* to ensure a successful shot.
*/
public boolean isReadyToLaunch() {
// 1. Check Flywheel RPM
// Math.abs() makes negative differences positive, so we just check the absolute distance!
boolean flywheelReady = Math.abs(getCurrentFlywheelRPM() - targetRPM) < TARGET_RPM_TOLERANCE;
// 2. Check Turret Alignment
boolean turretReady = Math.abs(getTurretPosition() - targetTurretPos) < TURRET_POSITION_TOLERANCE;
// 3. Check Hood Alignment
boolean visorReady = Math.abs(getHoodPosition() - targetVisorPos) < VISOR_POSITION_TOLERANCE;
// The subsystem is ONLY ready if all three mechanisms are at their targets!
return flywheelReady && turretReady && visorReady;
}
public double getCurrentFlywheelRPM() {
// (Returns current RPM from encoders)
return 0;
}
public double getTurretPosition() { return 0; }
public double getHoodPosition() { return 0; }
}Why put this in the Subsystem?
We put the isReadyToLaunch() method inside the Subsystem, not the Command. This is good Object-Oriented Programming.
Now, when you write your AutoFireCommand, the isFinished() method is incredibly clean and easy to read:
// Inside AutoFireCommand.java
@Override
public boolean isFinished() {
// The command is finished when the subsystem says it's ready!
return launchSubsystem.isReadyToLaunch();
}Persisting State Across OpModes
Another advanced control technique our team uses is Cross OpMode Storage.
When an FTC Autonomous period ends, the robot's code entirely stops and clears its memory. When you press INIT for the TeleOp period, everything starts from scratch.
But what if your turret ended the Auto period pointing backwards? The TeleOp code doesn't know that! When you press Play, the turret might aggressively snap back to 0, breaking the robot.
To fix this, we store the state in a static class that survives between OpModes:
package org.firstinspires.ftc.teamcode.common;
// This class stores variables that survive between Autonomous and TeleOp!
public class CrossOpModeStorage {
public static double turretPosition = 0.5; // Default center position
}Then, whenever the subsystem moves the turret, it saves the position to the storage:
// Inside LaunchSubsystem.java
public void setTurretPosition(double position) {
this.targetTurretPos = position;
turretServo.setPosition(position);
// Save it for later!
CrossOpModeStorage.turretPosition = position;
}Finally, when the Robot class initializes during TeleOp, it reads the saved position so it knows where the turret already is!
Check Your Understanding
Check yourself