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!");
  }
}

Reveal

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?

Reveal

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

This command is supposed to rotate the robot 90 degrees clockwise from its starting position.

Command rotateInPlace(double angleDegrees) {
  double targetAngle = imu.getRotation2d().getDegrees() + angleDegrees;
  return run(coroutine -> {
        while (imu.getRotation2d().getDegrees() < targetAngle) {
          differentialDrive.arcadeDrive(0.0, 0.2);
          coroutine.yield();
        }
      })
      .named("RotateInPlace");
}

Can you spot the error?

Reveal

The target angle is read once, before the command starts running. If the robot’s heading changes before the command runs, it will rotate to the wrong angle. Read the target angle from the IMU when the command starts running instead:

Command rotateInPlace(double angleDegrees) {
  return run(coroutine -> {
        double targetAngle = imu.getRotation2d().getDegrees() + angleDegrees;
        while (imu.getRotation2d().getDegrees() < targetAngle) {
          differentialDrive.arcadeDrive(0.0, 0.2);
          coroutine.yield();
        }
      })
      .named("RotateInPlace");
}