Example: Drive to Tag

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. No odometry needed.

1/**
2 * The robot's pose in the tag's frame, or null when the camera isn't looking at our tag.
3 *
4 * <p>Limelight target space: +X is the tag's right, +Y is down, +Z points out of the tag face. So
5 * distance is Z, sideways is X, squareness is the rotation about Y. TODO: verify the signs on
6 * hardware.
7 */
8private static Pose3d readRobotInTag(String limelightName, int targetTagId) {
9  // Also false when the camera is unplugged.
10  if (!LimelightHelpers.getTV(limelightName)) {
11    return null;
12  }
13  if ((int) LimelightHelpers.getFiducialID(limelightName) != targetTagId) {
14    return null;
15  }
16  Pose3d pose = LimelightHelpers.getBotPose3d_TargetSpace(limelightName);
17  // All zeros means no target-space data yet.
18  return pose.equals(Pose3d.kZero) ? null : pose;
19}

Three checks, and any one failing means there is no usable reading. The ID and the pose are published separately, so some frames carry the right ID and a pose of all zeros. Driving on those would mean driving at a tag underneath the robot.

Do not delete the ID check. The camera reports one tag at a time, and without it the robot drives at whichever tag becomes primary. No error, no warning.

Distance, sideways offset, and squareness are independent, so each gets its own ProfiledPIDController. That class is new here: a trapezoid profile and a PID controller in one object. All three are locals in create(...), and the coroutine body closes over them.

The tolerances are 3 cm and 2 degrees. setTolerance sets the band atGoal() reads, which is how the command decides it has arrived.

• enableContinuousInput goes on the heading controller only. Angles wrap. Without it, a controller sent from +3.0 rad to -3.0 rad takes the long way round instead of the 0.28 rad shortcut.
• ApplyRobotVelocity, not ApplyFieldVelocity. This command has no idea where the field is. It knows forward and left as the robot sees them.

Above the loop, setPriorityTagID pins the camera to our tag. Then reset(...) plants each profile at the current measurement, so the first pass ramps instead of lurching. Everything below runs once per robot loop.

When the helper returns null, the loop sends new SwerveRequest.Idle(), yields, and looks again. It does not guess and it does not give up. That is why the loop is while (true) with a break in the middle: the finish condition is not always answerable.

1  // Which number is which is explained on readRobotInTag below.
2  double measuredDistance = robotInTag.getZ();
3  double measuredLateral = robotInTag.getX();
4  double measuredYaw = robotInTag.getRotation().getY();
5 
6  // Back off to the standoff, slide until centered, turn until square.
7  double forward =
8      distance.calculate(measuredDistance, standoffMeters)
9          + distance.getSetpoint().velocity;
10  double sideways =
11      lateral.calculate(measuredLateral, 0.0) + lateral.getSetpoint().velocity;
12  double turn = heading.calculate(measuredYaw, 0.0) + heading.getSetpoint().velocity;
13 
14  // TODO: if the robot slides or turns the wrong way, flip that value's sign.
15  drivetrain.setControl(
16      driveRequest.withVelocity(new ChassisVelocities(forward, sideways, turn)));

Each speed is a plan plus a correction. getSetpoint().velocity is what the profile planned for this instant; calculate(...)is the PID output on top. Two of the goals are zero, because centered and square are both zero in the tag's frame. The third is the standoff, since zero distance is inside the tag.

The break sits after those three calculate(...) calls, on distance.atGoal() && lateral.atGoal() && heading.atGoal(). Move it above them and you are asking a controller with no measurement whether it has arrived.

There is no camera in simulation, so the helper returns null every pass and the command idles forever. That is the correct result, and it still checks the binding, the requirements, and the guard clause.

1. Run gradlew simulateJava and Enable. Drive with the left stick, then hold X and keep pushing. You should see: the robot stops dead and the sticks do nothing. Release X and they work again.
2. Put the robot on blocks, with a printed tag two meters away. Confirm the Limelight dashboard reports the right ID before you enable.
3. 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.
4. Cover the camera with your hand. You should see: the wheels stop within a loop or two, and start again when you uncover it.
5. 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.