01 | CHASE A MOVING POINT

A stop-and-settle waypoint route reaches one target before starting the next. Pure pursuit instead steers toward a point a little further along the path, then updates that point as the robot moves.

Imagine riding a bicycle through a bend. Looking just at the front tyre makes steering twitchy. Looking too far ahead can cut the corner. A robot faces the same trade-off.

Pure pursuit is a geometric path follower. It needs a reliable current pose and a prepared path. It does not discover obstacles or plan a safe route around them.

02 | PATH POINTS ARE NOT STOP POINTS

Represent the path as ordered field X/Y points, in millimetres. Adjacent points form straight segments. The follower can keep moving across intermediate points; only the endpoint requests a settled stop.

PATH = [(0, 0), (0, 600), (300, 900), (600, 900)]

This path goes north, diagonally northeast, then east. The robot starts at (0, 0), heading 0°. A polyline corner is sharp, while the robot’s steering arc is curved, so expect some corner cutting.

Use a broad, open test route. This starter supports simple forward paths without loops, self-intersections or nearby returning branches. It does not guarantee passing through every vertex.

03 | SELECT A LOOKAHEAD POINT

Draw a circle around the robot’s estimated reference point. Its radius is the lookahead distance. Find where the upcoming path crosses that circle and choose an intersection ahead in path order.

For a robot at (0, 0), a straight northbound path and a 300 mm lookahead, the target is (0, 300). After advancing to (0, 100), the target becomes (0, 400).

Lookahead distance is a geometric distance, not a delay in seconds. Near the endpoint, the remaining path may lie inside the circle. Then use the endpoint and its actual distance for the curvature calculation.

04 | KEEP PATH PROGRESS LOCAL

A circle can intersect a path more than once. Choosing a globally nearest point or the last intersection anywhere on the route can jump to the wrong branch.

The starter projects the robot onto a local forward path window, keeps cumulative progress from moving backwards and searches for the first forward circle intersection within twice the lookahead distance along the path.

This is a deliberately bounded selection policy. It may stop with NO_LOOKAHEAD instead of recovering from a large deviation. It is not a general solution for crossing paths.

05 | CONVERT THE TARGET INTO THE ROBOT FRAME

Keep the field convention from earlier guides: +X right, +Y up the field, and heading increasing clockwise from +Y. For steering, convert the target into robot-relative coordinates.

dx = target_x - robot_x
dy = target_y - robot_y
h = math.radians(heading_deg)
right = dx * math.cos(h) - dy * math.sin(h)
forward = dx * math.sin(h) + dy * math.cos(h)

At heading 0°, field-right is robot-right. At heading 90°, field +X is forward and field +Y is to the robot’s left.

06 | CURVATURE DESCRIBES HOW TIGHTLY TO TURN

Curvature is the reciprocal of turning radius. A straight line has zero curvature. A small radius means a large curvature magnitude.

actual_distance_squared = right**2 + forward**2
curvature = 2 * right / actual_distance_squared

With these axes, positive curvature means clockwise steering and negative means anticlockwise. The formula describes an arc tangent to the robot’s current forward direction and passing through the target.

Use the actual target distance squared. Do not always divide by LOOKAHEAD_MM squared, especially when the endpoint is closer than the circle radius.

For the right-hand example, distance squared is 100² + 300² = 100,000 mm². Curvature is 200 / 100,000 = 0.002 per mm. Radius is 1 / 0.002 = 500 mm.

07 | TURN CURVATURE INTO WHEEL SPEEDS

For an ideal differential drive with effective width T and signed centre speed v, clockwise angular speed is v × curvature.

left_speed  = v * (1 + curvature * T / 2)
right_speed = v * (1 - curvature * T / 2)

Positive curvature makes the left wheel faster, which turns the robot clockwise. With T = 300 mm and curvature = 0.002 per mm, the factors are 1.3 on the left and 0.7 on the right.

The teaching code applies those factors to a 15% centre request: left 19.5%, right 10.5%. PERCENT is a motor velocity request, not a measured centre speed in mm/s. The ratio approximates the geometry only with matched wheel sizes, gearing and drive response.

If an output exceeds the cap, scale both by the same factor to preserve their ratio. A sufficiently tight requested arc can reverse the inside side even though the path travel is forward.

DRIVE_TRACK_MM describes the drivetrain’s effective left-to-right width. It is separate from the offsets or spacing of passive odometry wheels. Scrub and slip can require physical calibration.

08 | TUNE LOOKAHEAD AND SPEED TOGETHER

Begin with fixed lookahead and a low cruise request. Change one value at a time. Record actual path error, endpoint error and repeatability.

An advanced option increases lookahead with measured speed, with minimum and maximum bounds. That is an extension, not a feature hidden in this fixed-lookahead starter.

09 | COMPLETE FORWARD PURE PURSUIT STARTER

Place this code below # Begin project code, outside the generated configuration region. Retain the generated VEX imports and Brain. Avoid duplicate Devices declarations.

Drive motors: left ports 3/4, right 1/2. Forward tracker: 17. Sideways tracker: 19. Inertial: 20. Verify reversals, tracker signs, scales, signed offsets and physical start before setting MOTION_CONFIGURED to True.

The inherited two-wheel estimator uses the same robot reference point as the path follower. For the differential-drive curvature model, that reference should be the drivetrain centre. Rebase an offset pose first.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER] | FORWARD PURE PURSUIT STARTER
# =============================================================================
# Keep generated VEX imports and brain above '# Begin project code'.
# Manual device configuration; do not duplicate declarations in Devices.
# Drive left 3/4, right 1/2, blue cartridges; verify reversals on your robot.
# Two perpendicular trackers: forward 17, sideways 19; inertial 20.
# One callback owns pose updates AND drivetrain output; no odometry thread.
import math
left_1 = Motor(Ports.PORT3, GearSetting.RATIO_6_1, False)
left_2 = Motor(Ports.PORT4, GearSetting.RATIO_6_1, False)
right_1 = Motor(Ports.PORT1, GearSetting.RATIO_6_1, True)
right_2 = Motor(Ports.PORT2, GearSetting.RATIO_6_1, True)
left_drive = MotorGroup(left_1, left_2)
right_drive = MotorGroup(right_1, right_2)
forward_tracker = Rotation(Ports.PORT17, False)
side_tracker = Rotation(Ports.PORT19, False)
inertial = Inertial(Ports.PORT20)
controller = Controller(PRIMARY)

MOTION_CONFIGURED = False
FORWARD_MM_PER_SENSOR_TURN = 219.44 # Calibrated examples only
SIDE_MM_PER_SENSOR_TURN = 219.44
FORWARD_X_MM = -120.0
SIDE_Y_MM = -100.0
START_POSE = (0.0, 0.0, 0.0)       # Physically confirmed autonomous start
DISTANCE_KP = 0.05                 # percent/mm
TURN_KP = 0.7                      # percent/degree
MAX_DRIVE = 20.0
MAX_TURN = 12.0
MAX_WHEEL_OUTPUT = 25.0
DRIVE_ALIGN_DEG = 12.0             # Rotate only outside this window
POSITION_TOL_MM = 20.0
HEADING_TOL_DEG = 2.0
POSITION_RATE_TOL = 30.0           # estimated field speed mm/s
TURN_RATE_TOL = 8.0                # degrees/s
SETTLE_MS = 200
POLL_MS = 20
MAX_GAP_MS = 100
ready = False
pose_valid = False
pose = None
previous_sensors = None
last_update_ms = 0

class FieldPose:
    def __init__(self, x_mm, y_mm, heading_deg):
        if not all(math.isfinite(v) for v in (x_mm, y_mm, heading_deg)):
            raise ValueError("Invalid starting pose")
        self.x = x_mm
        self.y = y_mm
        self.theta = math.radians(heading_deg)  # Keep unwrapped internally.

    def integrate(self, forward_arc, sideways_arc, dtheta):
        if not all(math.isfinite(v) for v in (forward_arc, sideways_arc, dtheta)):
            raise ValueError("Invalid local increment")
        half = dtheta / 2.0
        # Stable sin(half)/half limit near a straight move.
        factor = (1.0 - half * half / 6.0 if abs(half) < 1e-6
                  else math.sin(half) / half)
        forward_chord = forward_arc * factor
        side_chord = sideways_arc * factor
        midpoint = self.theta + half
        dx = side_chord * math.cos(midpoint) + forward_chord * math.sin(midpoint)
        dy = forward_chord * math.cos(midpoint) - side_chord * math.sin(midpoint)
        new_x, new_y = self.x + dx, self.y + dy
        new_theta = self.theta + dtheta
        if not all(math.isfinite(v) for v in (new_x, new_y, new_theta)):
            raise ValueError("Invalid resulting pose")
        self.x, self.y, self.theta = new_x, new_y, new_theta
        return dx, dy

    def pose(self):
        return self.x, self.y, math.degrees(self.theta) % 360.0

def clamp(value, low, high):
    return max(low, min(high, value))

def angle_error(target, current):
    return (target - current + 180) % 360 - 180

def target_geometry(x, y, target_x, target_y):
    dx, dy = target_x - x, target_y - y
    distance = math.sqrt(dx * dx + dy * dy)
    bearing = None if distance < 1e-6 else math.degrees(math.atan2(dx, dy)) % 360
    return distance, bearing

# =============================================================================
# SENSOR / MOTOR ACCESS
# =============================================================================
def stop_drive():
    left_drive.stop(BRAKE)
    right_drive.stop(BRAKE)

def side_output(group, output):
    if output == 0:
        group.stop(BRAKE)
    else:
        group.spin(FORWARD if output > 0 else REVERSE, abs(output), PERCENT)

def sensor_snapshot():
    if not inertial.installed() or inertial.is_calibrating():
        raise ValueError("Inertial unavailable")
    values = (forward_tracker.position(TURNS), side_tracker.position(TURNS),
              inertial.rotation(DEGREES))
    if not all(math.isfinite(v) for v in values):
        raise ValueError("Invalid sensors")
    return values

def initialize_pose():
    global pose, previous_sensors, last_update_ms, pose_valid
    pose_valid = False
    # Only at the physically verified stationary START_POSE.
    values = sensor_snapshot()
    pose = FieldPose(*START_POSE)
    previous_sensors = values
    last_update_ms = brain.timer.time(MSEC)
    pose_valid = True

def read_pose():
    global previous_sensors, last_update_ms, pose_valid
    if not pose_valid:
        raise ValueError("Pose invalid")
    now = brain.timer.time(MSEC)
    try:
        if now - last_update_ms > MAX_GAP_MS:
            raise ValueError("Pose update gap")
        current = sensor_snapshot()
        f = (current[0] - previous_sensors[0]) * FORWARD_MM_PER_SENSOR_TURN
        s = (current[1] - previous_sensors[1]) * SIDE_MM_PER_SENSOR_TURN
        theta = math.radians(current[2] - previous_sensors[2])
        pose.integrate(f + FORWARD_X_MM * theta, s - SIDE_Y_MM * theta, theta)
        previous_sensors = current
        last_update_ms = now
        return pose.pose()
    except Exception:
        pose_valid = False
        raise

# =============================================================================
# PATH GEOMETRY: FIELD +X RIGHT, +Y UP; CLOCKWISE HEADING
# =============================================================================
LOOKAHEAD_MM = 300.0
DRIVE_TRACK_MM = 300.0       # Effective drivetrain width, not tracker spacing
CRUISE_PERCENT = 15.0
PATH_TIMEOUT_MS = 14000
PATH = [(0.0, 0.0), (0.0, 600.0), (300.0, 900.0), (600.0, 900.0)]

def prepare_path(points):
    if len(points) < 2:
        raise ValueError("Need at least two path points")
    checked, cumulative = [], [0.0]
    for point in points:
        if len(point) != 2 or not all(math.isfinite(v) for v in point):
            raise ValueError("Use finite X/Y pairs")
        checked.append((float(point[0]), float(point[1])))
        if len(checked) > 1:
            a, b = checked[-2], checked[-1]
            length = math.hypot(b[0]-a[0], b[1]-a[1])
            if length < 1.0:
                raise ValueError("Remove duplicate or very close path points")
            cumulative.append(cumulative[-1] + length)
    return checked, cumulative

def local_progress(points, cumulative, x, y, progress, window):
    # Project only onto the next local path window, never the whole route.
    best_distance, best_s = float("inf"), progress
    for i in range(len(points)-1):
        lo = max(progress, cumulative[i])
        hi = min(progress + window, cumulative[i+1])
        if hi < lo:
            continue
        a, b = points[i], points[i+1]
        dx, dy = b[0]-a[0], b[1]-a[1]
        length = cumulative[i+1]-cumulative[i]
        t = ((x-a[0])*dx + (y-a[1])*dy) / (length*length)
        t = clamp(t, (lo-cumulative[i])/length, (hi-cumulative[i])/length)
        px, py = a[0]+t*dx, a[1]+t*dy
        distance = math.hypot(px-x, py-y)
        candidate_s = cumulative[i]+t*length
        if distance < best_distance:
            best_distance, best_s = distance, candidate_s
    return max(progress, best_s), best_distance

def circle_target(points, cumulative, x, y, progress, radius):
    # Solve |A + t(B-A) - robot|^2 = radius^2 for each local segment.
    # Choose the first forward intersection in path order.
    candidates = []
    for i in range(len(points)-1):
        if cumulative[i+1] < progress or cumulative[i] > progress+2*radius:
            continue
        a, b = points[i], points[i+1]
        dx, dy = b[0]-a[0], b[1]-a[1]
        fx, fy = a[0]-x, a[1]-y
        aa = dx*dx + dy*dy
        bb = 2*(fx*dx + fy*dy)
        cc = fx*fx + fy*fy - radius*radius
        discriminant = bb*bb - 4*aa*cc
        if discriminant < -1e-6:
            continue
        root = math.sqrt(max(0.0, discriminant))
        for t in ((-bb-root)/(2*aa), (-bb+root)/(2*aa)):
            if 0 <= t <= 1:
                path_s = cumulative[i] + t*math.sqrt(aa)
                if progress+1e-6 < path_s <= progress+2*radius:
                    candidates.append((path_s, a[0]+t*dx, a[1]+t*dy))
    if candidates:
        chosen = min(candidates)
        return chosen[1], chosen[2]
    end = points[-1]
    # Near the end the path may finish inside the lookahead circle.
    if (cumulative[-1]-progress <= 2*radius
            and math.hypot(end[0]-x, end[1]-y) <= radius):
        return end
    return None

def body_target(current, target):
    x, y, heading = current
    dx, dy = target[0]-x, target[1]-y
    h = math.radians(heading)
    right = dx*math.cos(h) - dy*math.sin(h)
    forward = dx*math.sin(h) + dy*math.cos(h)
    return right, forward

def pursuit_output(current, target, base, track):
    right, forward = body_target(current, target)
    distance_squared = right*right + forward*forward
    if distance_squared < 1.0 or forward <= 0:
        raise ValueError("Lookahead is too close or not ahead")
    curvature = 2*right/distance_squared  # clockwise-positive, 1/mm
    left = base*(1 + curvature*track/2)
    right_output = base*(1 - curvature*track/2)
    scale = max(1.0, abs(left)/MAX_WHEEL_OUTPUT,
                abs(right_output)/MAX_WHEEL_OUTPUT)
    return left/scale, right_output/scale, curvature

# =============================================================================
# BOUNDED FORWARD FOLLOWER; ONE OWNER FOR POSE AND DRIVE
# =============================================================================
def follow_path(points):
    stop_drive()
    try:
        points, cumulative = prepare_path(points)
        if (not all(math.isfinite(v) for v in
                    (LOOKAHEAD_MM, DRIVE_TRACK_MM, CRUISE_PERCENT, PATH_TIMEOUT_MS))
                or LOOKAHEAD_MM <= POSITION_TOL_MM or DRIVE_TRACK_MM <= 0
                or not 0 < CRUISE_PERCENT <= MAX_WHEEL_OUTPUT
                or PATH_TIMEOUT_MS <= 0):
            return "INVALID_CONFIGURATION"
        progress = 0.0
        started = previous_ms = brain.timer.time(MSEC)
        previous_pose, settled_since = None, None
        while True:
            if (not ready or not MOTION_CONFIGURED or not competition.is_enabled()
                    or not competition.is_autonomous() or controller.buttonB.pressing()):
                return "CANCELLED_OR_NOT_READY"
            now = brain.timer.time(MSEC)
            if now-started >= PATH_TIMEOUT_MS:
                return "TIMEOUT"
            current = read_pose()
            if previous_pose is None and math.hypot(
                    current[0]-points[0][0], current[1]-points[0][1]) > POSITION_TOL_MM:
                return "START_POSITION_MISMATCH"
            progress, error = local_progress(points, cumulative, current[0],
                                             current[1], progress, LOOKAHEAD_MM)
            if error > 1.5*LOOKAHEAD_MM:
                return "PATH_LOST"
            end_distance = math.hypot(points[-1][0]-current[0],
                                      points[-1][1]-current[1])
            near = (cumulative[-1]-progress <= POSITION_TOL_MM
                    and end_distance <= POSITION_TOL_MM)
            dt = (now-previous_ms)/1000.0
            slow = False
            if previous_pose is not None and dt > 0:
                speed = math.hypot(current[0]-previous_pose[0],
                                   current[1]-previous_pose[1])/dt
                rate = abs(angle_error(current[2], previous_pose[2]))/dt
                slow = speed <= POSITION_RATE_TOL and rate <= TURN_RATE_TOL
            previous_pose, previous_ms = current, now
            if near:
                stop_drive()
                if slow:
                    if settled_since is None:
                        settled_since = now
                    if now-settled_since >= SETTLE_MS:
                        return "SETTLED"
                else:
                    settled_since = None
            else:
                settled_since = None
                target = circle_target(points, cumulative, current[0], current[1],
                                       progress, LOOKAHEAD_MM)
                if target is None:
                    return "NO_LOOKAHEAD"
                right, forward = body_target(current, target)
                if forward <= 0:
                    return "NEEDS_ALIGNMENT"
                # Basic slowdown only. More complete final approach is in 07.5.
                base = min(CRUISE_PERCENT, DISTANCE_KP*end_distance)
                left, right, curvature = pursuit_output(current, target,
                                                         base, DRIVE_TRACK_MM)
                side_output(left_drive, left)
                side_output(right_drive, right)
            wait(POLL_MS, MSEC)
    except Exception:
        return "PATH_POSE_OR_COMMAND_FAULT"
    finally:
        stop_drive()

# =============================================================================
# COMPETITION CALLBACKS AND STATIONARY DISABLED SETUP
# =============================================================================
def autonomous():
    global pose_valid
    stop_drive()
    if not ready or not MOTION_CONFIGURED:
        return
    try:
        initialize_pose()  # Physically placed at START_POSE facing path entry.
        print("pure pursuit", follow_path(PATH))
    except Exception:
        print("STARTUP_POSE_FAULT")
    finally:
        stop_drive()
        pose_valid = False

def driver_control():
    global pose_valid
    stop_drive()  # Teaching example: no driver movement.
    pose_valid = False

competition = Competition(driver_control, autonomous)
stop_drive()
if MOTION_CONFIGURED and not competition.is_enabled() and inertial.installed():
    inertial.calibrate()  # Keep stationary; calibration may block.
    ready = not inertial.is_calibrating() and not competition.is_enabled()
while True:
    wait(100, MSEC)

One autonomous callback updates pose and owns motor output. The example has no separate odometry thread and no long blocking wait between movement samples.

The configured path starts ahead of the robot. If the selected target is behind or exactly sideways, the routine stops with NEEDS_ALIGNMENT. Align before entering the follower rather than forcing a forward steering arc.

10 | STOP ON FAILED FEEDBACK OR PATH SELECTION

A start estimate does not verify physical placement. A settled estimate does not prove physical arrival if odometry has drifted. Measure actual positions separately.

The basic endpoint slowdown does not guarantee a particular stopping distance or final heading. Those are the focus of 07.5. This example has no obstacle avoidance, dedicated stall detector or automatic recovery.

11 | BACKWARDS PATHS NEED AN EXPLICIT DESIGN

07.3 used reverse waypoint segments. This starter intentionally follows forward paths only. Negating the base request while leaving a front-facing target policy unchanged is not a complete reverse follower.

A reverse design must select points ahead in the direction of travel, handle the rearward travel frame, use signed speed consistently and stop or realign at direction changes. Test its steering signs independently before mixing directions.

12 | COMMON MISTAKES

• Using field X directly: transform the target into robot coordinates first.
• Copying a sign convention: this guide uses clockwise-positive heading and curvature.
• Using a fixed denominator: use the actual chord distance, especially near the end.
• Clipping each wheel separately: proportional scaling better preserves the requested ratio.
• Using tracker spacing as drive width: they describe different geometry.
• Ignoring the robot footprint: the chassis and mechanisms sweep beyond the reference-point path.
• Assuming pure pursuit removes drift: an inaccurate pose still gives inaccurate steering.

CHALLENGES | LOOK AHEAD, STEER AND MEASURE

1. Calculate: find curvature and radius for targets (0, 400), (80, 240) and (−80, 240) in the robot frame.
2. Transform: place the robot at (100, 200), heading 90°, with target (400, 300). Find right and forward.
3. Mix: use track width 300 mm and centre request 15% for the target (80, 240). Apply a 25% wheel cap.
4. Simulate: move the robot along a straight path and confirm that the target advances.
5. Compare: test lookaheads 200, 300 and 400 mm on one clear route at the same low cruise setting.
6. Inspect: log target position, progress, curvature and output at a bend.
7. Fault: simulate an unavailable target, a behind-robot target and a stale pose. Confirm no false success.
8. Evaluate: explain why a narrow corner may need a different route or a stop-and-turn action.

OFFICIAL REFERENCES

• Coulter: Implementation of the Pure Pursuit Path Tracking Algorithm (CMU report)
• Nav2 regulated pure pursuit documentation: advanced extensions
• VEX motor and motor group API

The VEX program is an independently written educational starter, not an implementation of Nav2 or a hardware-validated competition controller.