Three-Wheel Odometry

advanced55 min

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

This code snippet demonstrates how to update the robot’s X, Y, and heading using three encoders:

// 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

This snippet shows how to structure your op mode loop for odometry:

@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.

Implement the position update logic in Java.

                  public class OdometryPose {
    public double x, y, heading;
    public OdometryPose(double x, double y, double heading) {
        this.x = x; this.y = y; this.heading = heading;
    }
}

public OdometryPose updatePose(
    int prevLeft, int prevRight, int prevCenter,
    int currLeft, int currRight, int currCenter,
    double prevX, double prevY, double prevHeading,
    double trackWidth, double centerWheelOffset
) {
    double leftDelta = currLeft - prevLeft;
    double rightDelta = currRight - prevRight;
    double centerDelta = currCenter - prevCenter;
    double forwardMovement = (leftDelta + rightDelta) / 2.0;
    double headingChange = (rightDelta - leftDelta) / trackWidth;
    double lateralMovement = centerDelta - (headingChange * centerWheelOffset);
    double newX = prevX + forwardMovement * Math.cos(prevHeading) - lateralMovement * Math.sin(prevHeading);
    double newY = prevY + forwardMovement * Math.sin(prevHeading) + lateralMovement * Math.cos(prevHeading);
    double newHeading = prevHeading + headingChange;
    return new OdometryPose(newX, newY, newHeading);
}

Test your function with sample encoder values.

                  You can test your updatePose method with sample values. For example:

OdometryPose pose = updatePose(0, 0, 0, 1000, 1000, 0, 0, 0, 0, 15.0, 7.5);
System.out.println("X: " + pose.x + " Y: " + pose.y + " Heading: " + Math.toDegrees(pose.heading));
// This should show a forward movement with no rotation or lateral movement.

Verify the output by making the robot create a simple "S" shape.

                  To verify, simulate a sequence of encoder values that would make the robot move forward, turn, and then move forward again in the opposite direction. After each update, print the pose. You should see the X, Y, and heading values change in a way that traces an "S" path.

Put it all together: Modular Odometry Update Example

                  public class OdometryPose {
    public double x, y, heading;
    public OdometryPose(double x, double y, double heading) {
        this.x = x; this.y = y; this.heading = heading;
    }
}

public OdometryPose updatePose(
    int prevLeft, int prevRight, int prevCenter,
    int currLeft, int currRight, int currCenter,
    double prevX, double prevY, double prevHeading,
    double trackWidth, double centerWheelOffset
) {
    double leftDelta = currLeft - prevLeft;
    double rightDelta = currRight - prevRight;
    double centerDelta = currCenter - prevCenter;
    double forwardMovement = (leftDelta + rightDelta) / 2.0;
    double headingChange = (rightDelta - leftDelta) / trackWidth;
    double lateralMovement = centerDelta - (headingChange * centerWheelOffset);
    double newX = prevX + forwardMovement * Math.cos(prevHeading) - lateralMovement * Math.sin(prevHeading);
    double newY = prevY + forwardMovement * Math.sin(prevHeading) + lateralMovement * Math.cos(prevHeading);
    double newHeading = prevHeading + headingChange;
    return new OdometryPose(newX, newY, newHeading);
}

// In your op mode loop:
OdometryPose pose = new OdometryPose(0, 0, 0);
int prevLeft = 0, prevRight = 0, prevCenter = 0;
while (opModeIsActive()) {
    int currLeft = leftEncoder.getCurrentPosition();
    int currRight = rightEncoder.getCurrentPosition();
    int currCenter = centerEncoder.getCurrentPosition();
    pose = updatePose(prevLeft, prevRight, prevCenter, currLeft, currRight, currCenter, pose.x, pose.y, pose.heading, TRACK_WIDTH, CENTER_WHEEL_OFFSET);
    telemetry.addData("X", pose.x);
    telemetry.addData("Y", pose.y);
    telemetry.addData("Heading", Math.toDegrees(pose.heading));
    telemetry.update();
    prevLeft = currLeft; prevRight = currRight; prevCenter = currCenter;
}