Odometry

Choose a coordinate convention#

In this guide, +X points right on the field drawing, +Y points up, and heading θ is counterclockwise from +X. Distances are inches and internal angles are radians. Robot-local forward follows its nose; local sideways is positive to the robot’s left.

VEX inertial rotation is clockwise-positive by default. Negate its reading before converting to radians. This is intentionally different from the clockwise heading targets in the simple turn helpers. setPose() aligns the inertial reading with your chosen field orientation.

References: VEX Robotics: Inertial Sensor — C++ API (opens in a new tab) · Purdue SIGBots / Team 5225A: Odometry (opens in a new tab)

Choose the sensors#

Drive motor encoders can provide a first estimate of forward travel, but wheel slip appears as movement even when the robot does not travel that far. Unpowered tracking wheels measure their own ground contact and can improve that estimate; they still need consistent contact and accurate dimensions.

The example uses one forward rolling tracker and one sideways tracker, both with external rotation sensors. Use continuous position(), not angle(), so a complete wheel revolution is not lost at wraparound. Reverse each tracker as needed so pushing forward or left produces a positive reading.

References: VEX Robotics: Rotation Sensor — C++ API (opens in a new tab) · LemLib: Configuration (opens in a new tab)

Mount and wire the tracking wheels#

Use two unpowered omni wheels: one rolls with forward travel, and the other rolls sideways. Each wheel needs its own rotation sensor. The example assumes a direct 1:1 connection between the wheel shaft and sensor, with both wheels the same diameter. Geared trackers or different wheel sizes need separate conversion factors.

Support each shaft so the wheel spins freely without wobbling. Secure the sensor body so it cannot turn with the shaft. Keep both wheels in contact with the tiles throughout normal driving; a spring-loaded mount can help maintain contact. Check that a tracker does not lift when the intake or arm moves.

Connect the forward tracker to Smart Port 7 and the sideways tracker to Port 8 for the example. Route cables away from gears and moving shafts. These are example ports; update the constructors if your wiring differs.

References: VEX Robotics: Rotation Sensor — C++ API (opens in a new tab) · LemLib: Configuration (opens in a new tab)

Check wheel diameter and distance scale#

Start with the tracking-wheel diameter in inches, then test a measured straight movement on the field. The two-tracker example uses 2.75-inch wheels; replace trackingWheelDiameter with your wheel size. Do not use the drivetrain wheel diameter here.

Mark the robot’s start and end positions, push it straight a measured 24 inches, and compare the tracker’s reported travel. Correct direction before adjusting scale. If it reports 23.5 inches over a measured 24 inches, multiply the current diameter by 24 / 23.5. Repeat over several trials before changing the value.

Calibrate the forward and sideways trackers separately. If they need different effective diameters, give each tracker its own conversion. A large or inconsistent error is a reason to check wheel contact, shaft coupling, gearing, and measurement technique instead of hiding the error in a diameter value.

corrected diameter = current diameter × measured distance ÷ reported distance

References: VEX Robotics: Rotation Sensor — C++ API (opens in a new tab) · LemLib: Configuration (opens in a new tab)

Account for tracking-wheel offsets#

A tracker away from the robot’s turning center travels during an in-place rotation. That travel is not translation of the robot center. Measure the perpendicular offset from the turning center to each tracker’s measurement line.

With this guide’s signs, a forward tracker on the left reads negative travel during a counterclockwise turn. Add leftOffset × dθ to remove that rotational component. A sideways tracker ahead of center reads positive travel, so subtract forwardOffset × dθ. The signed example offsets are 3 inches left and 2 inches forward; replace them with measurements.

forward arc = Δforward + left offset × Δθ
sideways arc = Δsideways − forward offset × Δθ

References: Purdue SIGBots / Team 5225A: Odometry (opens in a new tab)

Before running odometry#

1. Push the robot forward: only the forward tracker should show substantial travel. Push left: only the sideways tracker should show substantial travel.
2. Change each rotation sensor’s reversed flag until forward travel and robot-left travel are positive. This guide’s sideways sign differs from LemLib’s documented robot-right convention.
3. Record the perpendicular left/right offset of the forward tracker and the front/back offset of the sideways tracker. Use the signs explained above.
4. Check that both trackers still touch the floor with the robot carrying its normal load.
5. Run the stationary tracking test below before enabling drive commands. An in-place turn should change heading while leaving the estimated center nearly fixed.

References: VEX Robotics: Rotation Sensor — C++ API (opens in a new tab) · Purdue SIGBots / Team 5225A: Odometry (opens in a new tab)

Update the pose#

Add these definitions above autonomous(). The update uses encoder differences, removes offset-induced rotation, converts local arcs into chords, and rotates them into the field frame using the midpoint heading. A small-angle limit avoids division by zero.

This derivation assumes roughly constant curvature within one sample. Run updates frequently enough that one sample does not contain a large, complex motion. Treat the resulting pose as an estimate, not a measurement of absolute field position.

rotation forwardTracker(PORT7, false);
rotation sideTracker(PORT8, false);
struct Pose { double x, y, theta; };
Pose pose{0, 0, 0};
double lastForward = 0, lastSide = 0, lastHeading = 0;
double headingOffset = 0;
constexpr double trackingWheelDiameter = 2.75;
// Offsets are inches left of center and forward of center.
constexpr double forwardWheelOffset = 3.0;
constexpr double sideWheelOffset = 2.0;

double trackerInches(rotation& sensor) {
  return sensor.position(degrees) / 360.0 * pi * trackingWheelDiameter;
}
double fieldHeading() {
  return -imu.rotation(degrees) * pi / 180.0 + headingOffset;
}
void setPose(double x, double y, double theta) {
  headingOffset = theta + imu.rotation(degrees) * pi / 180.0;
  pose = {x, y, theta};
  lastHeading = theta;
  lastForward = trackerInches(forwardTracker);
  lastSide = trackerInches(sideTracker);
}
void updateOdometry() {
  double theta = fieldHeading();
  double forwardNow = trackerInches(forwardTracker);
  double sideNow = trackerInches(sideTracker);
  double headingChange = theta - lastHeading;
  double forwardChange = forwardNow - lastForward;
  double sideChange = sideNow - lastSide;
  double forwardArc = forwardChange + forwardWheelOffset * headingChange;
  double sideArc = sideChange - sideWheelOffset * headingChange;
  double scale = std::fabs(headingChange) < 1e-6
    ? 1.0 : 2.0 * std::sin(headingChange / 2.0) / headingChange;
  double forwardChord = forwardArc * scale;
  double sideChord = sideArc * scale;
  double midHeading = lastHeading + headingChange / 2.0;
  pose.x += forwardChord * std::cos(midHeading) - sideChord * std::sin(midHeading);
  pose.y += forwardChord * std::sin(midHeading) + sideChord * std::cos(midHeading);
  pose.theta = theta;
  lastForward = forwardNow;
  lastSide = sideNow;
  lastHeading = theta;
}

References: Purdue SIGBots / Team 5225A: Odometry (opens in a new tab) · VEX Robotics: Rotation Sensor — C++ API (opens in a new tab)

Run one pose update loop#

Start with this stationary tracking test. It stops the motors, initializes a known pose, and prints updates for ten seconds while you move the robot by hand. Only this autonomous loop writes pose.

To integrate motion helpers, call updateOdometry() inside each drive/turn loop. Do not also run a second odometry task. If you later introduce tasks, give pose a single owner and route reset requests through it, or protect all shared accesses with an appropriate synchronization mechanism. Declaring pose volatile is not a synchronization strategy.

void autonomous() {
  if (!sensorsReady) { stopDrive(); return; }
  stopDrive();
  setPose(24, 24, 0);
  timer test;
  test.reset();
  while (test.time(msec) < 10000) {
    updateOdometry();
    Brain.Screen.setCursor(1, 1);
    Brain.Screen.print("x %.1f y %.1f deg %.1f   ",
      pose.x, pose.y, pose.theta * 180.0 / pi);
    wait(10, msec);
  }
}

References: PROS: Multitasking and synchronization (opens in a new tab)

Check for drift#

1. Push straight along a measured line. Check direction first, then distance scale.
2. Push sideways. Confirm the side tracker increases for robot-left motion.
3. Rotate about the tracking center without translating. X and Y should remain nearly fixed. A curved position trace usually points to offset signs, offset magnitudes, wheel lift, or heading scale.
4. Drive or push a square and compare the ending pose to the starting mark. Test on the actual field surface.
5. Check that setPose() refreshes the previous sensor samples. Otherwise the next update can reapply movement from before the reset.

References: LemLib: Configuration (opens in a new tab)