Driving to coordinates

Calculate the target bearing#

Subtract the current position from the target to get dx and dy. The distance is the vector length. atan2(dy, dx) returns a counterclockwise angle from +X in radians, matching this site’s odometry convention.

The order of atan2 arguments matters. A target directly above the robot should produce +90°; directly to its right should produce 0°. Do these simple checks before commanding a motor.

dx = target X − pose X
dy = target Y − pose Y
distance = sqrt(dx² + dy²)
target heading = atan2(dy, dx)

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

Combine drive and turn output#

This controller uses counterclockwise-positive heading error. A positive correction therefore makes the left side slower and the right side faster. This is the opposite sign arrangement from the earlier clockwise-positive turn helper.

Reduce forward output when the robot faces away from the target. A cosine multiplier, clamped to zero when misaligned by more than 90°, lets the robot turn before advancing. Normalize the mixed commands together to keep their proportions within the output limit.

left = drive − turn
right = drive + turn

References: Purdue SIGBots: PID Controller (opens in a new tab)

A basic drive-to-point function#

Add this after the odometry definitions and before autonomous(). It updates the pose within the same loop that commands the drivetrain. Initialize pose at the actual starting location and orientation before the first movement. For a robot at (24, 24) facing +X, use setPose(24, 24, 0), then call driveToPoint(48, 36) inside autonomous().

The example only drives forward and finishes inside a one-inch radius for about 150 ms. It does not guarantee a final heading, plan around obstacles, or generate a smooth path. The gains and tolerance are examples to tune, and friction may prevent a small command from moving the robot.

bool driveToPoint(double targetX, double targetY,
                  double timeoutMs = 3500) {
  if (!sensorsReady || !std::isfinite(targetX)
      || !std::isfinite(targetY) || timeoutMs <= 0) {
    stopDrive(); return false;
  }
  timer elapsed;
  elapsed.reset();
  int settledMs = 0;
  while (elapsed.time(msec) < timeoutMs) {
    updateOdometry();
    if (!std::isfinite(pose.x) || !std::isfinite(pose.y)
        || !std::isfinite(pose.theta)) { stopDrive(); return false; }
    double dx = targetX - pose.x;
    double dy = targetY - pose.y;
    double remaining = std::sqrt(dx * dx + dy * dy);
    if (remaining < 1.0) {
      stopDrive();
      settledMs += 10;
      if (settledMs >= 150) return true;
    } else {
      settledMs = 0;
      double targetHeading = std::atan2(dy, dx);
      double headingError = wrap180(
        (targetHeading - pose.theta) * 180.0 / pi);
      double turn = limit(0.7 * headingError, -30, 30);
      double alignment = std::max(0.0,
        std::cos(headingError * pi / 180.0));
      double drive = limit(4.0 * remaining, 0, 40) * alignment;
      double left = drive - turn, right = drive + turn;
      double scale = std::max(1.0,
        std::max(std::fabs(left), std::fabs(right)) / 40.0);
      leftDrive.spin(forward, left / scale, pct);
      rightDrive.spin(forward, right / scale, pct);
    }
    wait(10, msec);
  }
  stopDrive();
  return false;
}

References: VEX Robotics: Motor and Motor Group — C++ API (opens in a new tab)

Point targets and pose targets#

A point specifies X and Y. A pose adds a desired final heading. Reaching the right point while facing the wrong way can still make the next intake or scoring action fail.

LemLib documents separate move-to-point and move-to-pose behavior. Those are useful concepts to compare, but the library uses PROS. The function here is an original VEXcode example and does not implement LemLib’s algorithms or options.

References: LemLib: Lateral Motion (opens in a new tab)

Test short targets first#

1. Set a known pose and verify the displayed coordinates before driving.
2. Try a short target on +X, then +Y, then behind the starting heading.
3. Test what happens when the robot starts inside the target radius.
4. Keep the route clear. This controller does not detect or avoid obstacles.
5. If travel is inaccurate, test odometry separately before retuning the point controller.