Finish Conditions

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

Hand it a lambda. () -> robot.armflywheel.isAtTarget() is a question the scheduler can ask on every loop. Drop the () -> and the method runs on the spot, passing one frozen answer. The build stops on boolean cannot be converted to BooleanSupplier.

Like Command.sequence(...), .until(...) returns a builder, and .named("...") closes it.

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

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()robot.flywheel.runFast() is a hold. It re-sends its request every loop and never finishes, which suits a held button and is useless in 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));

1import static org.wpilib.units.Units.Seconds;
2 
3Command spinUp =
4    robot.flywheel.runFast()
5        .until(() -> robot.flywheel.isAtTarget())
6        .named("runFast until at target")
7        .withTimeout(Seconds.of(2.0));

The hold itself is untouched. The condition is the ending you want; the timeout is the ending you get when a sensor dies or the armflywheel jams. It goes after .named(...), because .withTimeout(...) is a method on Command, not on the builder.

Raise the arm, wait for it to arrive, then spin the flywheel while the arm keeps holding. A sequence cannot do that, because each member has to finish before the next starts, so the arm's hold would end first.

The shape for it is a coroutine. Its body is ordinary Java, read top to bottom, and it can pause on a line and carry on from there. This is a first look; Coroutines, next, takes it apart.

1. Give MyTeleop a robot field. The routine body is a method of its own, outside the constructor, and the constructor's robot parameter does not reach it. Add private final Robot robot; beside driver, and make this.robot = robot; the first line of the constructor.
2. Import org.wpilib.command3.Command and org.wpilib.command3.Coroutine.
3. Add the Y binding to the constructor and the method below it to the class.

1// Y: raise the arm, then spin the flywheel once it is really there.
2driver
3    .y()
4    .whileTrue(
5        Command.noRequirements(coroutine -> spinUpWhenReady(coroutine))
6            .named("Spin Up When Ready (hold)"))
7    .whileFalse(robot.flywheel.stop());
8 
9// ... and, further down the class:
10 
11private void spinUpWhenReady(Coroutine coroutine) {
12  // fork, not await: vertical() is a hold and never finishes.
13  coroutine.fork(robot.arm.vertical());
14 
15  coroutine.waitUntil(() -> robot.arm.isAtTarget());
16 
17  // runFast is a hold too, so this never returns: releasing Y cancels the whole routine.
18  coroutine.await(robot.flywheel.runFast());
19}

A condition that cannot go true is as bad as a bare hold. The step never ends, nothing throws, nothing logs, and the armflywheel keeps pushing. Fifteen seconds of autonomous go on step one.

The timeout covers the first two and cannot help with the third. For a armflywheel 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 raiseArmspinUp 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 armflywheel's tolerance to Degrees.of(0.001)RotationsPerSecond.of(0.001) and press it again.
4. Leave the tolerance broken, drop .withTimeout(...), and press it once more.
5. Put the tolerance back and hold Y. The arm goes up, and the flywheel starts only once the arm is there.

Write down the tolerance you settled on and how long the step took. Double that time for a backstop that will not fire on a good run. Coroutines asks for that number.