LESSON 30

Align to Tag

Hold a button and the robot lines up on a spot measured from one AprilTag. It never asks where it is on the field, so a wrong field pose cannot move the spot. You record the spot on each field before the event.

Branchswerve-align-to-tag14 minutes

You’ll need

Drive to Point. This lesson is the same call with a different pose.
Vision, working on the real robot. There is no camera in the simulator.
An AprilTag on a wall at scoring height, and room to drive two meters out from it.

Drive to Point arrives where the field pose says the spot is. If the pose is off by 10 cm, so is the robot. Scoring usually happens in front of one tag, and the camera can see that tag the whole way in. Measure the spot from that tag and the field pose drops out of the problem.

The tag's frame

A pose measured from a tag uses the tag's own axes. X points straight out of its face and Y to the tag's left. The heading is 180° when the robot squarely faces the tag. A spot 1 m out, centered, facing the tag is new Pose2d(1.0, 0.0, Rotation2d.PI).

The Limelight reports the same thing in its own convention, called target space. X points right as you face the tag, Y down, and Z into the wall. toTagFrame turns one into the other.

toTagFrame

1private static Pose2d toTagFrame(Pose3d robotInTargetSpace) {
2  return new Pose2d(
3      -robotInTargetSpace.getZ(),
4      robotInTargetSpace.getX(),
5      // Turning about the downward Y axis is turning clockwise seen from above.
6      Rotation2d.PI.minus(new Rotation2d(robotInTargetSpace.getRotation().getY())));
7}

DANGER · SIGNS

Test this first

A wrong sign here sends the robot the wrong way at full speed. Run the first check below with the robot on blocks or disabled before anything drives.

The tag map

The camera answers a few dozen times a second and late, and it loses the tag when the robot gets close. The wheels answer every loop. So each sighting puts the tag down once, in the same frame as drivetrain.getPose(). Between sightings the wheels say where the robot is from that spot.

samplePoseAt looks up where the robot was when the picture was taken. Adding the tag's offset from the robot to that pose places the tag. The call goes before the trust gates in addFrame, because one close tag is exactly the sighting those gates throw out.

The tag map, rememberTags and robotInTag

1// Where each tag sat in the drivetrain's pose frame the last time a camera saw it, by tag ID.
2private final Map<Integer, Pose2d> tagPoses = new HashMap<>();
3 
4private void rememberTags(LimelightResults frame, double timestampSeconds) {
5  Optional<Pose2d> poseThen = drivetrain.samplePoseAt(timestampSeconds);
6  if (poseThen.isEmpty()) {
7    return;
8  }
9  for (FiducialTarget tag : frame.fiducialTargets) {
10    Pose2d robotInTag = toTagFrame(tag.getRobotPose_TargetSpace());
11    // The tag's own origin, seen from the robot, added to where the robot was.
12    Pose2d tagPose = poseThen.get().plus(new Transform2d(robotInTag, Pose2d.ZERO));
13    tagPoses.put(tag.fiducialId, tagPose);
14    Telemetry.getTable("Vision").log("RobotInTag" + tag.fiducialId, robotInTag);
15  }
16}
17 
18public Optional<Pose2d> robotInTag(int tagId) {
19  Pose2d tagPose = tagPoses.get(tagId);
20  if (tagPose == null) {
21    return Optional.empty();
22  }
23  return Optional.of(drivetrain.getPose().relativeTo(tagPose));
24}

The first line of addFrame gets the call, right after the MegaTag1 estimate: rememberTags(frame, estimate.timestampSeconds);. DriveMechanism gets a one-line samplePoseAt that passes the call to the drivetrain.

Drive in the tag's frame

BLine follows whatever pose it is handed. Give it a builder whose pose comes from vision.robotInTag(tagId) and it drives in the tag's frame, with the same one-waypoint path as Drive to Point.

alignToTag, under driveToPoint

1public Command alignToTag(Vision vision, int tagId, Pose2d spotInTag) {
2  FollowPath align =
3      new FollowPath.Builder(
4              DriveType.SWERVE,
5              this,
6              () -> vision.robotInTag(tagId).orElseThrow(), // where the robot is, from the tag
7              pose -> {}, // never reset odometry from a tag-relative pose
8              () -> getRobotVelocity(),
9              velocities -> driveRobotRelative(velocities),
10              new PIDController(2.0, 0.0, 0.0),
11              new PIDController(3.0, 0.0, 0.0),
12              new PIDController(0.2, 0.0, 0.0))
13          // The tag's frame has no alliance side, so there is nothing to turn around.
14          .withShouldFlip(() -> false)
15          .build(new Path(new Path.Waypoint(spotInTag)));
16 
17  return Command.noRequirements(
18          coroutine -> {
19            if (vision.robotInTag(tagId).isEmpty()) {
20              DataLogManager.log("AlignToTag: tag " + tagId + " not seen yet");
21              return;
22            }
23            coroutine.await(align);
24          })
25      .named("AlignToTag");
26}

The pose reset does nothing, because a pose measured from a tag is not a field pose. If no camera has seen the tag, the check before the await ends the command at once. The robot never drives on an empty answer. The gains are the ones from BLine Paths.

The binding, under the Drive to Point bindings

1    // Hold X to line up 1 m straight out from tag 7, facing it.
2    driver
3        .x()
4        .whileTrue(drivetrain.alignToTag(robot.vision, 7, new Pose2d(1.0, 0.0, Rotation2d.PI)));

Record your spot

Do not measure the spot with a tape. Record it with the camera that will drive to it, and its mounting error goes into the number and back out again.

Disable the robot and push it by hand to exactly where it should score, with the camera on the tag.
Read Vision/RobotInTag7 in AdvantageScope or Elastic: X, Y and the heading in degrees.
Type them into the binding, converting the heading with Rotation2d.fromDegrees(...), and deploy.

Record it again on the competition field at the event, before the first match. Field crews mount tags by hand, so no two fields match to the centimeter. A spot recorded on that field is right on that field.

Check your work

Disabled, push the robot square to tag 7, about 1.5 m out, and read Vision/RobotInTag7. Push it toward its own right, then turn it a little to the left.
Park the robot 2 to 3 m from the tag at an angle. Enable Teleop and hold X.
Cover the camera, push the robot a meter sideways so the field pose is wrong, uncover it, and hold X again.

Check

You should see

About (1.5, 0, 180°) square on. Y growing as the robot moves to its right, and the heading wrapping from 180° to about −175° as it turns left.
The robot drive to 1 m out and turn to face the tag, then stop on its own.
The same stopping spot the second time, wrong field pose or not.

The command ends at once and nothing moves. No camera has seen that tag ID. Check the number on the tag and that Vision/RobotInTag7 is updating.

The first check reads the wrong way. A Y that shrinks as the robot moves right, or a heading that grows as it turns right, is a sign flipped in toTagFrame. Fix it there and run the first check again. Do not drive on it.