Distance Resets

The Problem: Odometry Drift

Dead-reckoning odometry (covered in the previous chapter) accumulates error every time the robot moves. After 20–30 seconds of movement, the robot's internally-tracked position can be 1–2 inches off from its real position.

In a match auton (15 seconds) this is usually acceptable. In a 60-second skills run targeting 75+ points, each scoring position must be accurate to within half an inch. A 1.5" positional error means the robot misses the goal entirely.

The Core Idea

The VEX field walls are at a fixed, known distance from the field center. On a standard field, each wall is approximately 70.4375 inches from the origin in both the X and Y directions.

If we mount a distance sensor on the robot and point it at a wall, we can measure the distance from the sensor to the wall. Combined with the known wall position and the sensor's mounting offset from the robot's center, we can calculate the robot's exact position in that axis.

In practice, the robot is never perfectly parallel to the wall, so we need to apply a cosine correction to account for the angle. More on that shortly.

Sensor Placement

This robot mounts two distance sensors:

The Math

Simple case: robot perfectly facing the wall

Assume the robot is heading 0° (facing +Y / North) and we have a forward-facing distance sensor measuring distance to the North wall (at Y = +70.4375"):

Where:

• wallY = 70.4375 inches (known constant)
• distance = distance sensor reading in inches
• sensorOffset = distance from sensor to robot center (positive if sensor is in front of center)

Real case: robot at an angle to the wall

When the robot is not perfectly perpendicular to the wall, the distance sensor beam hits the wall at an angle. This makes the measured distance longer than the true perpendicular distance.

The correction factor is |cos(θ)| where θ is the angle between the sensor beam and the wall normal. For a forward-facing sensor at heading θ:

The sensor offset from the robot center must also be scaled the same way, because the sensor itself is displaced at the same angle:

And for a right-facing sensor (which measures along the X axis when heading is 90°), you add π/2 to the heading before taking the cosine:

The Implementation

The Chassis class in include/systems/chassis.hpp extends lemlib::Chassis with two new methods: tareYPos and tareXPos.

tareYPos — Reset the Y coordinate

include/systems/chassis.hppvoid tareYPos(float wallInchesFromOrigin,   // ①
              pros::Distance *localizer,         // ②
              float sensorOffset,               // ③
              float scaleFactor = 1,            // ④
              float sensorRotationOffset = 0.0f) { // ⑤
  float robotY = wallInchesFromOrigin;

  // ⑥ Take multiple readings and average them for accuracy
  std::vector<float> measuredDistance = {};
  int numberOfScans = 3;
  while (true) {
    numberOfScans--;
    measuredDistance.push_back(localizer->get_distance() * 0.0393701 * scaleFactor); // ⑦
    if (numberOfScans == 0) break;
    pros::delay(25);
  }

  // Average readings
  float sum = 0.0f;
  for (float d : measuredDistance) sum += d;
  float averageDistance = sum / measuredDistance.size();

  // ⑧ Apply cosine correction and sensor offset
  if (wallInchesFromOrigin < 0) {          // South wall
    robotY = robotY
           + fabs(cos(getPose(true).theta + sensorRotationOffset)) * averageDistance
           - fabs(cos(getPose(true).theta + sensorRotationOffset)) * sensorOffset;
  } else {                                   // North wall
    robotY = robotY
           - fabs(cos(getPose(true).theta + sensorRotationOffset)) * averageDistance
           + fabs(cos(getPose(true).theta + sensorRotationOffset)) * sensorOffset;
  }

  // ⑨ Apply the corrected Y, keep X and heading unchanged
  this->setPose(this->getPose().x, robotY, this->getPose().theta);
}

tareXPos — Reset the X coordinate

Identical logic to tareYPos, but corrects X instead of Y. The cosine uses heading + π/2 (the sensor rotation is shifted by 90° relative to Y-axis sensors) and setPose updates X while preserving Y and heading.

include/systems/chassis.hpp// Key difference: the rotation offset is shifted by π/2
fabs(cos(getPose(true).theta + (sensorRotationOffset + std::numbers::pi / 2)))

// And we update X, not Y:
this->setPose(robotX, this->getPose().y, this->getPose().theta);

Using Distance Resets in Autons

Distance resets are inserted at natural waypoints in the auton where:

• The robot is near a wall (sensor range is short and accurate)
• The robot has just stopped or is moving slowly (avoids measurement noise)
• The robot is approximately perpendicular to the wall (minimizes cosine correction error)

Call syntax used in skills

src/autons/autons.cpp// Reset X position using distanceBack (facing the West wall at −70.4375")
chassis.tareXPos(-70.4375, &distanceBack, -2.75, 1.00);

// Reset Y position using distanceRight (facing South wall at −70.4375")
// M_PI/2 offset because the sensor faces right (90° from forward)
chassis.tareYPos(-70.4375, &distanceRight, -4.75, 1.00, M_PI/2);

Placement strategy in the skills run

In the skills auton, resets happen whenever the robot turns to face a wall before moving to a scoring position. A typical sequence:

src/autons/autons.cpp  (skills)// After clearing the park zone, robot is near the corner.
// Turn to face the nearest walls and reset both axes.
chassis.turnToHeading(90, 10000, {.minSpeed = 20}, false);
chassis.tareXPos(-70.4375, &distanceBack,  -2.75, 1.00);        // ← X reset
chassis.tareYPos(-70.4375, &distanceRight, -4.75, 1.00, M_PI/2); // ← Y reset

// Now navigate to goal — odometry is corrected, moves will be accurate.
chassis.turnToPoint(-1.65 * TILE_UNIT, -2 * TILE_UNIT, 10000);
chassis.moveToPoint(-1.65 * TILE_UNIT, -2 * TILE_UNIT, 10000);

Bonus: GPS as a Fallback Reset

Before distance sensors were used, the robot used a VEX GPS sensor for position correction. The GPS sensor reads field-tape-based visual landmarks to get an absolute position. The tareGps method in chassis.hpp implements this:

include/systems/chassis.hppvoid tareGps(pros::Gps *gps, bool heading = false) {
  pros::gps_position_s_t position = gps->get_position();
  double x = position.x * 39.37008; // meters → inches
  double y = position.y * 39.37008;
  double theta = gps->get_heading();

  // Optionally also reset the heading from GPS
  this->setPose(x, y, heading ? theta : this->getPose().theta);
}

GPS works well when the field tape is clean and the sensor has line-of-sight. However, it failed during an early skills run this season, which is why distance sensors are now the primary correction method.

Practical Tips