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 robot.arm::isAtTarget, the condition passes the method itself, so it can be called again on every loop. Add the parentheses and robot.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 final Angle tolerance = Degrees.of(1.0);
2 
3/** Where the arm is now, straight off the CANcoder. */
4public Angle getPosition() {
5  return encoder.getPosition().getValue();
6}
7 
8/** Where the last position request asked it to go. */
9public Angle getTargetPosition() {
10  return positionOut.getPositionMeasure();
11}
12 
13/** True when the arm has reached its target angle. */
14public boolean isAtTarget() {
15  return getPosition().isNear(getTargetPosition(), tolerance);
16}

1private final AngularVelocity tolerance = RotationsPerSecond.of(0.5);
2 
3/** How fast the wheel is turning now, straight off the motor. */
4public AngularVelocity getVelocity() {
5  return motor.getVelocity().getValue();
6}
7 
8/** The speed the last velocity request asked for. */
9public AngularVelocity getTargetVelocity() {
10  return velocityOut.getVelocityMeasure();
11}
12 
13/** True when the flywheel has reached its target speed. */
14public boolean isAtTarget() {
15  return getVelocity().isNear(getTargetVelocity(), tolerance);
16}

robot.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    robot.arm.vertical()
5        .until(robot.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            robot.arm.vertical()
4                .until(robot.arm::isAtTarget)
5                .named("raise arm")
6                .withTimeout(Seconds.of(2.0)),
7            robot.flywheel.runFast().withTimeout(Seconds.of(1.0)),
8            robot.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. robot.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 and Flywheel, then bind raiseArm to a button with onTrue in your MyTeleop. It is a step now, so it ends itself.
2. Press it once and time how long the step takes to end.
3. Change the arm's tolerance to Degrees.of(0.001) 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.