Lesson 32: Integrating Dead Reckoning with Drive Control

Dead reckoning becomes more useful when drive control can use it. Instead of only saying "drive forward," code can say "drive toward this estimated position" or "hold this heading while moving."

Store a Pose

A pose is the robot's estimated position and direction. A simple pose can have x, y, and heading.

public class RobotPose {
    public double x;
    public double y;
    public double headingDegrees;
}

Update the Pose

Each cycle, compare the new encoder distance to the previous distance. The difference is how far the robot moved since the last update.

double currentDistance = drive.getAverageDistance();
double deltaDistance = currentDistance - lastDistance;
lastDistance = currentDistance;

double heading = Math.toRadians(gyro.getAngle());
pose.x += deltaDistance * Math.cos(heading);
pose.y += deltaDistance * Math.sin(heading);
pose.headingDegrees = gyro.getAngle();

Use Pose for Driving

Once you estimate pose, you can compare the robot's current position to a target position. The difference becomes an error, just like PID control.

double xError = targetX - pose.x;
double yError = targetY - pose.y;
double distanceError = Math.sqrt(xError * xError + yError * yError);

Reset When You Know Better

If the robot touches a wall, starts a match in a known location, or sees a trusted vision target, you can reset the estimate. Good robot code combines dead reckoning with real-world reference points whenever possible.

Practice

Write pseudocode for an autonomous route that starts at x=0, y=0, drives to x=60, y=0, then turns to 90 degrees. Include when you reset encoders and gyro.