I’m working on an FTC TeleOp where a motorized spindexer (with encoder + PID) advances to shoot game pieces. After shooting, I want the spindexer to automatically return to encoder position 0.
I understand that I probably need to use state flags, but I’m not sure where or how to structure them in my control loop.
What I’m trying to do
Press right stick → spindexer advances to shoot (PID-controlled)
After the shot finishes & shot=true → spindexer should rotate back to encoder position 0 (targert=0)
While this happens, I don’t want:
the PID loop to be overridden immediately
the return-to-zero command to interrupt the shooting motion
Current issue
I already have a shot boolean flag, which I use to switch the behavior of the right stick / B button.
However, if I reuse this flag to also trigger the return-to-zero logic, the motor doesn’t wait for the shooting motion to finish — it immediately overrides the current target.
I tried doing something like:
if (!sorter.isBusy()) {
target = 0;
arm.setPosition(0.81);
}
I learned that isBusy() only works with RUN_TO_POSITION mode so it can't work.
Here's the full relevent code:
@TeleOp
public class sorter extends LinearOpMode {
static final int LOAD = 155;
static final int STEP_TICKS = 191;
static final int BACK_STEPS = 7;
static final int TOLERANCE = 3;
static double MAX_POWER = 0.3;
boolean shot = false;
PIDController pid = new PIDController(0.004 , 0.0005, 0.0);
int target = 0;
DcMotor sorter;
Servo arm;
@Override
public void runOpMode() {
sorter = hardwareMap.get(DcMotor.class, "sorter");
arm = hardwareMap.get(Servo.class, "arm");
sorter.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
sorter.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
waitForStart();
while (opModeIsActive()) {
if (gamepad1.bWasPressed()) {
if (!shot) {
shot = true;
MAX_POWER = 0.3;
Thread t = new Thread(() -> {
try {
target -= LOAD - 20;
pid.reset(getRuntime());
arm.setPosition(0.81);
Thread.sleep(450);
target += LOAD - 10;
pid.reset(getRuntime());
} catch (InterruptedException ignored) {}
});
t.start();
} else {
MAX_POWER = 0.85;
shot = false;
target += BACK_STEPS * STEP_TICKS;
pid.reset(getRuntime());
}
}
int pos = sorter.getCurrentPosition();
int error = target - pos;
double power = pid.update(target, pos, getRuntime());
power = Math.max(-MAX_POWER, Math.min(MAX_POWER, power));
if (Math.abs(error) <= TOLERANCE) {
power = 0;
pid.reset(getRuntime());
}
sorter.setPower(power);
}
}
}
}
