Spot the Error, Part 2

Exercise 1

“Hello World!” doesn’t show up in the console while the a button is pressed. Can you spot the error?

public class Robot extends OpModeRobot {
  private final CommandXboxController xbox = new CommandXboxController(0);

  public Robot() {
    xbox.a().whileTrue(printHelloWorld());
  }

  @Override
  public void robotPeriodic() {}

  private Command printHelloWorld() {
    return Command.noRequirements(
            coroutine -> {
              while (true) {
                System.out.println("Hello World!");
                coroutine.yield();
              }
            })
        .named("Hello World!");
  }
}

Answer

The scheduler never runs, so scheduled commands never execute. Call Scheduler.getDefault().run() inside robotPeriodic():

@Override
public void robotPeriodic() {
  Scheduler.getDefault().run();
}

Exercise 2

This command is supposed to drive the robot using the joystick’s forward and rotation axes.

Command arcadeDrive(DoubleSupplier forwardThrottle, DoubleSupplier rotationThrottle) {
  double forward = forwardThrottle.getAsDouble();
  double rotation = rotationThrottle.getAsDouble();
  return run(coroutine -> {
        while (true) {
          differentialDrive.arcadeDrive(forward, rotation);
          coroutine.yield();
        }
      })
      .named("Drive");
}

Can you spot the error?

Answer

The supplier values are read once, before the command starts running. The robot drives at the same speed and rotation forever, even as the joystick moves. Read the values from the suppliers inside the loop instead:

Command arcadeDrive(DoubleSupplier forwardThrottle, DoubleSupplier rotationThrottle) {
  return run(coroutine -> {
        while (true) {
          differentialDrive.arcadeDrive(
              forwardThrottle.getAsDouble(), rotationThrottle.getAsDouble());
          coroutine.yield();
        }
      })
      .named("Drive");
}

Exercise 3: Extra Spice

We have the following code for an arm mechanism:

public class Arm implements Mechanism {
  private double target = 0;
  private final TalonFX motor = new TalonFX(0, CANBus.systemcore(0));
  private final Trigger hasNotHitTarget =
      new Trigger(() -> motor.getPosition().getValueAsDouble() < target);

  public Command subOptimalSetAngle(double targetRotations) {
    return run(coroutine -> {
          target = targetRotations;
          motor.setThrottle(0.1);
          while (hasNotHitTarget.getAsBoolean()) {
            coroutine.yield();
          }
          motor.setThrottle(0);
        })
        .named("Use PID instead(you'll learn this in Stage 1C!)");
  }
}

Assume that in your teleop opmode/robot class, you have the call xbox.a().whileTrue(arm.subOptimalSetAngle(0.5)). Also assume that the motor starts at a position of 0.1 rotations.

You hold down the A button, and the arm doesn’t move. You release the button and hold it down again, and the arm moves. Why?

Answer

Triggers only update at the beginning of every robot loop cycle. Before subOptimalSetAngle is called, the trigger hasNotHitTarget evaluates to false, since 0.1, the motor’s position, is not greater than target (which currently has a value of 0).

When the command is run, it sets target to 0.5, and immediately evaluates hasNotHitTarget.getAsBoolean() without waiting 1 loop. Because the trigger’s internal value hasn’t updated, the trigger still evaluates to false. So, the while loop is skipped and the motor throttle is immediately set back to 0.

On the second press, the same problem occurs; but this time, hasNotHitTarget is true since the trigger had time to update. So, the command runs normally.

To avoid this, simply add coroutine.yield() before the while loop. This effectively waits for 1 loop cycle before starting the while loop.