Implementing Odometry

Translating encoder changes into (x, y, θ) updates with a tiny bit of trig.

The algorithm (simplified)

Every 10 ms:

1. Read the left and right encoder positions.
2. Calculate how much each wheel moved since last update (ΔL and ΔR).
3. Average distance: d = (ΔL + ΔR) / 2.
4. Change in heading: Δθ = (ΔL − ΔR) / trackWidth.
5. Update heading: θ += Δθ.
6. Update position using the average heading during the move:
   • x += d * cos(θ - Δθ/2)
   • y += d * sin(θ - Δθ/2)

The C++ implementation

#include "vex.h"
#include <cmath>
// using namespace vex;  (auto-included by VEXcode IQ)

// --- Configure for your robot ---
const double WHEEL_DIAM_IN = 2.75;      // your wheel diameter
const double TRACK_WIDTH   = 11.0;      // distance between left and right wheels
const double TICKS_PER_REV = 360.0;     // motor encoder ticks per revolution

// --- Pose ---
double poseX = 0.0;
double poseY = 0.0;
double poseT = 0.0;   // theta in radians

// --- Previous encoder readings ---
double prevL = 0;
double prevR = 0;

double ticksToInches(double ticks) {
  double revs = ticks / TICKS_PER_REV;
  return revs * (3.14159 * WHEEL_DIAM_IN);
}

void odomUpdate() {
  double curL = LeftMotor.position(degrees);
  double curR = RightMotor.position(degrees);

  double dL = ticksToInches(curL - prevL);
  double dR = ticksToInches(curR - prevR);
  prevL = curL;
  prevR = curR;

  double d  = (dL + dR) / 2.0;          // forward distance moved
  double dT = (dL - dR) / TRACK_WIDTH;  // change in heading (radians)

  double thetaMid = poseT + dT / 2.0;   // midpoint heading
  poseX += d * cos(thetaMid);
  poseY += d * sin(thetaMid);
  poseT += dT;
}

// Run this in its own task so the main code is free
int odomTask() {
  while (true) {
    odomUpdate();
    wait(10, msec);
  }
  return 0;
}

int main() {
  // start the tracking task
  task tracker(odomTask);

  // ... your autonomous code, which can read poseX, poseY, poseT anytime
}

Tuning

• Track width, measure the distance between the centers of the left and right wheels. If your robot's heading is wrong, this is the first thing to adjust.
• Wheel diameter, if x and y are scaled wrong (e.g., 50% off), your diameter is wrong.
• Test, drive the robot in a 24-inch square. After 4 corners it should return to (0, 0) and θ=0. If not, tune.

Using the position

// Drive to a point on the field
void driveToPoint(double targetX, double targetY) {
  while (true) {
    double dx = targetX - poseX;
    double dy = targetY - poseY;
    double dist = sqrt(dx*dx + dy*dy);
    if (dist < 1.0) break;  // within 1 inch

    // P-control: aim at the target, then move toward it
    double targetHeading = atan2(dy, dx);
    double headingError  = targetHeading - poseT;
    // ... apply PID to turn while moving
    wait(20, msec);
  }
}

Limitations to be honest about

Two-wheel odometry using only drive encoders is the simplest setup, but it has weaknesses:

• Wheel slip destroys accuracy. Sharp turns and pushing matches cause drift.
• The gyro is more accurate for heading than encoder difference. Many teams use the gyro for θ and encoders just for distance.
• 3-wheel odometry (with two perpendicular tracking wheels not attached to the drivetrain) is the gold standard. Out of scope for IQ, common in V5.

Curated references

• readPurdue SIGBots: full Odometry derivation
• readGitHub: VEX Odometry Tutorial in C++ (reference code)
• videoVEX Auton Tutorial Pt. 2: Setting Up Odometry
• readVEX Forum: Position Tracking / Odometry Tutorial

void odomUpdate() {
  double curL = LeftMotor.position(degrees);
  double curR = RightMotor.position(degrees);

  double dL = ticksToInches(curL - prevL);
  double dR = ticksToInches(curR - prevR);
  // (no update of prevL / prevR here)

  double d  = (dL + dR) / 2.0;
  double dT = (dL - dR) / TRACK_WIDTH;

  poseX += d * cos(poseT + dT/2.0);
  poseY += d * sin(poseT + dT/2.0);
  poseT += dT;
}

void odomUpdate() {
  // ... encoders read and delta-ed correctly ...
  double d  = (dL + dR) / 2.0;
  double dT = (dL - dR) / TRACK_WIDTH;

  poseX += d * cos(poseT);   // using start heading
  poseY += d * sin(poseT);   // using start heading
  poseT += dT;
}