Three-Wheel Odometry

Why Use Three Wheels?

A three-wheel odometry setup allows your robot to track its position (X, Y) and heading (angle) on the field with high accuracy. By using two parallel wheels and one perpendicular wheel, you can resolve both translational and rotational movement.

Mathematics of Three-Wheel Odometry

Three-wheel odometry uses the change in encoder values from each wheel to calculate the robot's movement. The two parallel wheels measure forward/backward and rotational movement, while the perpendicular wheel measures side-to-side (lateral) movement.

For a detailed explanation, see gm0: Odometry.

Basic Position Update Logic

// Assume previous and current encoder values are stored
double leftDelta = currentLeft - previousLeft;
double rightDelta = currentRight - previousRight;
double centerDelta = currentCenter - previousCenter;

double forwardMovement = (leftDelta + rightDelta) / 2.0;
double headingChange = (rightDelta - leftDelta) / trackWidth;
double lateralMovement = centerDelta - (headingChange * centerWheelOffset);

// Update robot's position
robotX += forwardMovement * Math.cos(robotHeading) - lateralMovement * Math.sin(robotHeading);
robotY += forwardMovement * Math.sin(robotHeading) + lateralMovement * Math.cos(robotHeading);
robotHeading += headingChange;

Implementing Three-Wheel Odometry in Android Studio

In your FTC code, you will need to read encoder values each loop, calculate the deltas, and update your robot's position. This is typically done in a loop inside your op mode.

Sample Loop for Odometry Update

@Override
public void runOpMode() {
    // Initialize encoders and variables...
    waitForStart();
    while (opModeIsActive()) {
        // Read current encoder values
        int left = leftEncoder.getCurrentPosition();
        int right = rightEncoder.getCurrentPosition();
        int center = centerEncoder.getCurrentPosition();
        // Calculate deltas and update position (see above)
        // ...
        telemetry.addData("X", robotX);
        telemetry.addData("Y", robotY);
        telemetry.addData("Heading", Math.toDegrees(robotHeading));
        telemetry.update();
        // Store previous values for next loop
        // ...
    }
}

Calibration and Tuning

Accurate odometry requires careful calibration. Measure your track width (distance between parallel wheels) and center wheel offset precisely. Test your robot's movement and adjust these values as needed for best results. Additionally, if you have a CAD model of your robot, you can also use those distances for a more accurate measurement.

Common Pitfalls and How to Avoid Them

Common mistakes include swapping encoder directions, incorrect math, or mismeasured distances. Always verify your encoder directions and test with simple movements before running complex autonomous routines.

Practice Exercise

Write a function that takes in the previous and current encoder values for all three wheels and returns the robot's new X, Y, and heading.

Implement the position update logic in Java.
Test your function with sample encoder values.
Verify the output by making the robot create a simple "S" shape.

Open full interactive app