LESSON 34

Example: Drive to Tag

Hold X and the robot drives to a meter in front of an AprilTag, squares up to it, and stops. No odometry, no field map: the camera is the only sensor.

13 minutes

You’ll need

Coroutines: coroutine.yield() and a waitUntil with a time limit.
Vision: the LimelightLib vendordep installed, and your camera's name.
Profiled Drive to Point: trapezoid profiles, PID plus feedforward.

One new file, commands/DriveToTagInline.java, and one line in TeleopOpMode: driver.x().whileTrue(DriveToTagInline.create(robot.drivetrain, "limelight", 1, 1.0)). The arguments are the drivetrain, the camera's name, the tag ID, and the standoff in meters. Tag 1 is a placeholder.

The file imports com.limelightvision.Limelight and FiducialTarget from the same package, plus a static import of org.wpilib.units.Units.Seconds. VS Code offers the rest as you type.

The tag's frame

Every other drive command here works in field space, a Pose2d from the blue corner. This one works in target space, where the origin is the tag. LimelightLib uses the same axes in every space: X points out of the tag's face, Y to the tag's left, and Z up.

DriveToTagInline.java: the pose helper

1/** The robot's pose in our tag's frame, or null when the camera can't see that tag. */
2private static Pose3d readRobotInTag(Limelight camera, int targetTagId) {
3  // False when the camera is unplugged, its newest frame is stale, or it sees nothing.
4  if (!camera.hasTarget()) {
5    return null;
6  }
7  for (FiducialTarget tag : camera.getLatestResults().fiducialTargets) {
8    if (tag.fiducialId == targetTagId) {
9      return tag.getRobotPose_TargetSpace();
10    }
11  }
12  return null;
13}

A frame lists every tag in view, each with its own robot pose. The loop picks ours by ID and ignores the rest, so a second tag drifting into frame cannot pull the robot toward it. Leave the ID check in. Without it the robot drives at whichever tag comes first in the list.

hasTarget() matters because the getters never go blank. After a camera drops out they keep returning its last frame. Once the newest frame is a quarter second old, hasTarget() turns false. The helper then stops handing out a pose the robot has already driven past.

One controller per axis

DriveToTagInline.java: create(...), the setup

1public static Command create(
2    DriveMechanism drivetrain, String cameraName, int targetTagId, double standoffMeters) {
3  Limelight camera = new Limelight(cameraName);
4 
5  // One profiled PID per axis: the profile plans a smooth ramp, PID trims the drift.
6  // TODO: tune the speed limits and kP on your robot.
7  ProfiledPIDController distance =
8      new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2.5, 3.0));
9  ProfiledPIDController lateral =
10      new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2.5, 3.0));
11  ProfiledPIDController heading =
12      new ProfiledPIDController(
13          0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(Math.PI, 2.0 * Math.PI));
14 
15  SwerveRequest.ApplyRobotVelocity driveRequest =
16      new SwerveRequest.ApplyRobotVelocity()
17          .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
18 
19  heading.enableContinuousInput(-Math.PI, Math.PI);
20  distance.setTolerance(0.03); // meters
21  lateral.setTolerance(0.03); // meters
22  heading.setTolerance(Math.toRadians(2.0)); // radians

Distance, sideways offset and squareness are independent, so each gets its own ProfiledPIDController: a trapezoid profile and a PID controller in one object. All of them are locals, and the coroutine body below closes over them. setTolerance sets the band atGoal() reads, which is how the command decides it has arrived.

enableContinuousInput goes on the heading controller only. Angles wrap, and squared up sits right on the wrap, as the next section shows.
ApplyRobotVelocity, not ApplyFieldVelocity. This command has no idea where the field is. It knows forward and left as the robot sees them.

The Limelight made here is a second reader on the camera Vision already uses. It is safe. It only looks at the newest frame and never touches the queue Vision works through.

The loop

DriveToTagInline.java: create(...), the command

1  return drivetrain
2      .run(
3          coroutine -> {
4            boolean tracking = false;
5 
6            while (true) {
7              Pose3d robotInTag = readRobotInTag(camera, targetTagId);
8 
9              // No tag in view: stop, then give it a second to appear before giving up.
10              if (robotInTag == null) {
11                stop(drivetrain, driveRequest);
12                tracking = false;
13                if (coroutine
14                    .waitUntil(() -> readRobotInTag(camera, targetTagId) != null, Seconds.of(1.0))
15                    .timedOut()) {
16                  return;
17                }
18                continue;
19              }
20 
21              // First reading since the tag came into view. Start each profile here, at rest.
22              if (!tracking) {
23                distance.reset(robotInTag.getX());
24                lateral.reset(robotInTag.getY());
25                heading.reset(robotInTag.getRotation().getZ());
26                tracking = true;
27              }
28 
29              // Back off to the standoff, slide until centered, turn until square.
30              double forward =
31                  distance.calculate(robotInTag.getX(), standoffMeters)
32                      + distance.getSetpoint().velocity;
33              double sideways =
34                  lateral.calculate(robotInTag.getY(), 0.0) + lateral.getSetpoint().velocity;
35              double turn =
36                  heading.calculate(robotInTag.getRotation().getZ(), Math.PI)
37                      + heading.getSetpoint().velocity;
38 
39              // Facing the tag, the robot's forward is the tag's -X and its left is the tag's -Y.
40              drivetrain.setControl(
41                  driveRequest.withVelocity(new ChassisVelocities(-forward, -sideways, turn)));
42 
43              if (distance.atGoal() && lateral.atGoal() && heading.atGoal()) {
44                break;
45              }
46              coroutine.yield();
47            }
48 
49            stop(drivetrain, driveRequest);
50          })
51      .whenCanceled(() -> stop(drivetrain, driveRequest))
52      .named("DriveToTagInline");
53}

With no tag in view the loop stops the robot and waits up to a second for one. A tag that never shows up ends the command, stopped, rather than leaving a held button doing nothing forever. When a tag does show up, tracking is false, so each profile restarts from the robot's real position at rest. Without that, a profile picks up its old plan mid-ramp and the robot lurches.

Each speed is a plan plus a correction. getSetpoint().velocity is what the profile planned for this instant, and calculate(...) adds the PID output on top. Centered is zero, and the distance goal is the standoff. Squared up means facing the tag, so the robot points back down the tag's X axis. That is a yaw of half a turn, Math.PI, right on the wrap the heading controller was told about.

The minus signs come from the same fact. The robot faces the tag, so its forward and left run opposite to the tag's X and Y. The break sits after the three calculate(...) calls. Move it above them and you are asking a controller with no measurement whether it has arrived.

Watch out

Every gain ships at zero

All three controllers start with kP, kI and kD at 0.0, so calculate(...) returns zero and the profile does the whole job. That is a safer first run than an untuned gain. Nothing corrects error, though: when the profile runs out, 20 cm short stays 20 cm short.

A stop on every exit

DriveToTagInline.java: the stop helper

1/** Sends zero speed. Ending a command does not do this for you. */
2private static void stop(DriveMechanism drivetrain, SwerveRequest.ApplyRobotVelocity request) {
3  drivetrain.setControl(request.withVelocity(new ChassisVelocities()));
4}

The command can end three ways. It arrives and breaks out of the loop. It waits a second for a tag that never comes and returns. Or it is canceled, because the driver let go of X. The first two run a stop(...) in the body. A canceled body is dropped where it stands and runs nothing more, so .whenCanceled(...) calls the same helper.

The helper exists because ending a command does not stop a motor. In teleop the joystick default would cover for a missing stop. Schedule this from an autonomous OpMode, where the drivetrain has no default, and the robot keeps rolling at its last speed.

Check your work

There is no camera in simulation, so the helper returns null, the wait runs out after a second, and the command ends stopped. That still checks the binding and the requirement.

In simulation, enable Teleop and drive with the left stick, then hold X and keep pushing. You should see: the robot stops dead. A second later the command ends and the sticks work again, X still held.
Put the robot on blocks, with a printed tag two meters away. Confirm the Limelight web interface reports the right ID before you enable.
Enable and hold X. You should see: the wheels swing to an angle and spin. Wrong direction means a sign to flip, and blocks make that check free.
Cover the camera with your hand. You should see: the wheels stop within a few loops. Uncover it inside a second and they start again. Leave it covered and the command ends.
Signs right? On the floor, area clear, hold X. You should see: a ramp, a cruise, a slow-down, then a stop a meter out and square to the tag.

Watch out

If it did not work

Holding X stops the robot and nothing else. The helper returns null every pass. Either the camera name is wrong, the tag ID is wrong, or the camera cannot see the tag. The web interface settles which.

It drives away, slides sideways, or spins. That is a sign. Negate the one value that matches what the robot did, and only that one.

It stops short and never ends. The profile finished and there is no kP to close the last gap, so the measurement stays outside the 3 cm tolerance. Give distance and lateral a small kP.