Finish Conditions

.until(...) wraps a command and ends it on the first loop a condition comes back true. That condition is a BooleanSupplier: any small piece of code that answers true or false when it is asked. The scheduler asks about fifty times a second.

Hand it a method reference. Written as arm::isAtTarget, the condition passes the method itself, so it can be called again on every loop. Add the parentheses and arm.isAtTarget() runs the method on the spot, passing one frozen answer. That will not compile: boolean cannot be converted to BooleanSupplier.

.until(...) hands back a builder rather than a Command, the same way Command.sequence(...) did. .named("...") closes it. Leave the name off and the build fails, because a builder is not a Command.

The mechanism owns the comparison. Its units, its target, and its tolerance are already in that one file. Put the arithmetic there too, and every call site gets one readable question instead.

1private static final double POSITION_TOLERANCE_ROT = 0.01;
2 
3public boolean isAtTarget() {
4  double error = targetPositionRot - getPositionRot();
5  return Math.abs(error) <= POSITION_TOLERANCE_ROT;
6}

arm.vertical() is a hold. It re-sends its position request every loop and never finishes, so it suits a held button and is useless as a member of a list. One call site turns it into a step.

1import static org.wpilib.units.Units.Seconds;
2 
3Command raiseArm =
4    arm.vertical()
5        .until(arm::isAtTarget)
6        .named("vertical until at target")
7        .withTimeout(Seconds.of(2.0));

vertical() itself is untouched and still reusable anywhere. The condition is the ending you want. The timeout is the ending you get when a sensor dies or the arm jams. It goes after .named(...), since .withTimeout(...) is a method on Command and not on the builder.

A routine is those steps in order. Every member needs an ending, including the last one.

1Command score =
2    Command.sequence(
3            arm.vertical()
4                .until(arm::isAtTarget)
5                .named("raise arm")
6                .withTimeout(Seconds.of(2.0)),
7            flywheel.runFast().withTimeout(Seconds.of(1.0)),
8            flywheel.stop().withTimeout(Seconds.of(0.1)))
9        .named("Score");

The two flywheel members have no arrival to wait for, so their timeouts are the intended ending rather than a backstop. Spinning for one second is the instruction. flywheel.stop() is a hold too, so without a timeout on it the group never finishes either. A tenth of a second is long enough to send zero and release the mechanism. Autonomous stops its drivetrain the same way.

A condition that cannot go true is as bad as a bare hold. The sequence sits on that step, nothing throws, nothing logs, and the arm keeps pushing. A fifteen-second autonomous period spends all fifteen on step one.

Never true

Tolerance too tight

The arm settles half a degree outside the band and stops there. The plot looks fine. The routine does not move.

Never changes

A dead sensor

A CANcoder off the bus reports one value forever, so isAtTarget() gives the same answer every loop whatever the arm does.

True too early

Passing through

A fast mechanism crosses the target for one loop on its way past. The step ends while it is still moving.

The timeout covers the first two. It cannot help with the third. For a mechanism that overshoots, require the reading to stay inside tolerance for several loops in a row. Keep that behind the same isAtTarget(), so no call site changes.

Run it in the simulator, then break it on purpose. You are done when you can tell the two endings apart without watching a clock.

1. Add isAtTarget() to Arm, then bind raiseArm to a button with onTrue in your TeleopOpMode. It is a step now, so it ends itself.
2. Press it once and time how long the step takes to end.
3. Set POSITION_TOLERANCE_ROT to 0.0001 and press it again.
4. Leave the tolerance broken, drop .withTimeout(...), and press it once more.

Write down the tolerance you settled on and how long the step took at it. That time is the floor for any timeout on this arm. Double it and you have a backstop that will not fire on a good run.