Lab 11: Track the Robot on the Field (Odometry)#

Why it matters#

In Lab 8, the robot learned two things: how far each side has rolled (the encoders) and which way it's facing (the NavX2). Each answer is useful on its own. Put together, they answer a much bigger question: where is the robot on the field?

That's odometry. Every 20 ms, the robot asks "how far did I just roll, and which way was I facing?" and adds that small step to its position. Do that 50 times a second and you get a running estimate of the robot's pose: its X and Y position in meters, and its heading.

In this lab you'll add odometry to the West Coast Drive, watch a little robot move on a field map in Elastic as you drive, and calibrate it until the map matches the floor. Path following (Lesson 19) and vision (Lesson 18) both start from the pose you build here.

Before you start: finish Lab 8, including the encoders (Steps 8–12). Read Lesson 17 first. You'll add to the same WestCoastDrive project.

Read this#

• Lesson 17: Where Is the Robot?
• Differential Drive Odometry (WPILib)
• Field2d Widget (WPILib)

Walkthrough#

What you'll build#

The robot starts at (0, 0) facing 0°, wherever you set it down. Driving forward makes X go up. Driving to the robot's left makes Y go up. Turning left makes the heading go up, the same as the gyro in Lab 8.

Step 1: Measure the track width#

Odometry needs one new measurement: the track width, the distance between the left wheels and the right wheels, measured center of wheel to center of wheel, in meters.

Lab 8 already has a placeholder for it in Constants.java:

public static final double TrackWidthMeters = 0.6;

Measure your chassis with a tape measure and put the real number in. You'll fine-tune it in Step 9.

Step 2: Add the odometry and the field#

Open DriveSubsystem.java. Add these imports at the top with the others:

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

Then add two fields below the encoders:

  // Odometry: adds up encoder distances and gyro headings into a position on the field (Lab 11)
  private final DifferentialDriveOdometry m_odometry;

  // A field map for Elastic, with the robot drawn where odometry thinks it is
  private final Field2d m_field = new Field2d();

m_odometry is final but not set yet. Java lets you set a final field once, in the constructor. You'll do that next, because odometry needs the encoders to be set up first.

Step 3: Create the odometry in the constructor#

At the end of the constructor, after the two setDistancePerPulse lines, add:

    // Start odometry at (0, 0) facing 0°, using the sensors' current readings as the starting point
    m_odometry = new DifferentialDriveOdometry(
        m_gyro.getRotation2d(),
        getLeftMeters(),
        getRightMeters());

    // Show the field map in Elastic (it appears under SmartDashboard/Field)
    SmartDashboard.putData("Field", m_field);

Odometry remembers these first readings. From then on, it only cares how much they change. That's why it doesn't matter whether the encoders read 0 or 3.7 at startup.

Step 4: Update it every loop#

In periodic(), add these lines at the top, before the telemetry lines:

    // Odometry: add this loop's small step to the robot's position
    m_odometry.update(m_gyro.getRotation2d(), getLeftMeters(), getRightMeters());
    m_field.setRobotPose(getPose());

And at the bottom of periodic(), with the other telemetry:

    Pose2d pose = getPose();
    m_telemetry.log("Odometry/X (m)", pose.getX());
    m_telemetry.log("Odometry/Y (m)", pose.getY());
    m_telemetry.log("Odometry/Heading (deg)", pose.getRotation().getDegrees());
    m_telemetry.log("Odometry/Pose", pose);   // the whole pose, for AdvantageScope

periodic() runs every 20 ms whether or not a command is running, so odometry never misses a step.

Step 5: Read and reset the pose#

Add these two methods above periodic():

  /** Where odometry thinks the robot is: X and Y in meters, and heading. */
  public Pose2d getPose() {
    return m_odometry.getPoseMeters();
  }

  /**
   * Tells odometry the robot is at (0, 0) facing 0°, right where it is now.
   * The encoders and gyro aren't touched. Works while disabled.
   */
  public Command resetPoseCommand() {
    return runOnce(() -> m_odometry.resetPosition(
        m_gyro.getRotation2d(), getLeftMeters(), getRightMeters(), Pose2d.kZero))
        .ignoringDisable(true)
        .withName("Reset Pose");
  }

resetPosition needs the sensors' current readings so it knows the new starting point. Pose2d.kZero is (0, 0) facing 0°.

Step 6: Change the reset buttons#

In Lab 8, Back zeroed the gyro and Start reset the encoders. Both of those now cause trouble: if a sensor reading suddenly jumps to 0, odometry thinks the robot just moved or spun, and the robot on the field map jumps.

resetPoseCommand() doesn't touch the sensors, so use it instead. In RobotContainer.java, find these two lines in configureBindings():

    m_controller.back().onTrue(m_driveSubsystem.zeroHeadingCommand());

    m_controller.start().onTrue(m_driveSubsystem.resetEncodersCommand());

Delete both and put this one in their place:

    // Start: "the robot is at (0, 0) facing 0°" (Lab 11)
    m_controller.start().onTrue(m_driveSubsystem.resetPoseCommand());

The turn and distance commands from Lab 8 still work. They measure from wherever the sensors are when they start, so they never needed the sensors at 0.

Step 7: Put the field in Elastic#

Deploy, then open Elastic.

1. Right-click an empty spot → Add Widget, or open the widget list.
2. Find SmartDashboard → Field and drag it onto the layout. Elastic shows it as a Field widget: a picture of this year's field with a robot on it.
3. Make the widget big. Turn on the robot outline if it's off.

Also add Telemetry → Drive → Odometry → X (m), Y (m), and Heading (deg) as text displays, so you can see the numbers.

With the robot disabled, push it by hand. The robot on the map should move the same way. Press Start and it jumps back to the corner.

Step 8: Check the directions#

Enable and drive gently. Check each one:

If something goes the wrong way:

Step 9: Calibrate#

Odometry is only as good as its numbers. Two quick tests find most problems.

Wheel diameter (straight line). Put tape on the floor, line the robot up on it, and press Start. Drive straight for about 3 m and stop. Measure how far the robot really went with a tape measure.

If the ratio isn't close to 1.00, multiply WheelDiameterMeters by it. A 6 in wheel squishes under a heavy robot, so the "real" diameter is often a little smaller than 6 in. Run the test again until odometry is within about 2 cm of the tape.

Track width (spin in place). This checks TrackWidthMeters against the gyro, which you trust for turns. Press Start, then spin the robot in place to the left exactly 5 full turns (watch the gyro heading reach about 1800°). Read Encoders/Left (m) and Encoders/Right (m) from Lab 8 before and after.

effective track width = (Δ right − Δ left) ÷ (turns × 2π)

For 5 turns, divide by 5 × 2π ≈ 31.42.

On a skid-steer like the WCD, the effective number usually comes out bigger than the tape measure, because the wheels slide sideways while turning (you saw this in Lab 8, Step 11). Path following works better with the effective number, so write both down. You'll use the effective one in Lesson 19.

Step 10: Drive the sensor square#

Select Square in the Auto dropdown (since Lab 8 it drives by the encoders and gyro), set the robot on a tape mark, press Start, and run autonomous.

When it finishes, the robot should be back near (0, 0) on the map. Fill in:

Compare the map with the floor. If odometry says (0.02, 0.01) but the robot is 15 cm from the mark, odometry has drifted: the wheels slipped, and the encoders counted rolling that didn't move the robot.

Step 11: Commit#

git add .
git commit -m "Lab 11: odometry and field map"

When it doesn't work#

Piece by piece#

private final DifferentialDriveOdometry m_odometry;

m_odometry = new DifferentialDriveOdometry(
    m_gyro.getRotation2d(),
    getLeftMeters(),
    getRightMeters());

m_odometry.update(m_gyro.getRotation2d(), getLeftMeters(), getRightMeters());

m_field.setRobotPose(getPose());

m_telemetry.log("Odometry/Pose", pose);

m_odometry.resetPosition(
    m_gyro.getRotation2d(), getLeftMeters(), getRightMeters(), Pose2d.kZero)

Try it#

• Watch the robot on the field map while pushing the robot by hand
• Check all three rows of the direction table in Step 8
• Calibrate the wheel diameter until a 3 m drive is within 2 cm
• Measure the effective track width and compare it with the tape-measure number
• Run the sensor square 3 times and fill in the table
• Drive a figure eight in teleop and come back to the start mark. How far off is odometry?
• Challenge: add resetPoseCommand(Pose2d pose) that sets any pose, and start the robot at (2, 4) facing 180°
• Challenge: write driveToXCommand(double x) that drives until the pose's X reaches a value

Check yourself#

Parts of this lab are adapted from the Odometry and Pose Estimation Calibration lessons of Mantik by Itkan Robotics (CC BY 4.0), rewritten for the RoboJacks West Coast Drive.