01 | WAYPOINTS TURN A MOVE INTO A ROUTE

A waypoint is a named position the robot should reach, with optional final heading and movement settings. A route is an ordered sequence of these waypoints.

This guide uses stop-and-settle navigation: align, approach, settle, optionally orient, then advance. It is not a continuous path follower.

Place waypoints where the robot needs a deliberate turn, orientation or clearance decision. The next guide introduces smoother lookahead following.

02 | DEFINE DIRECTION PER INCOMING SEGMENT

reverse=True describes travel toward that waypoint, not an instruction to flip its coordinates. The field target remains unchanged.

Retreating from the approach point moves toward field-down while the robot keeps facing field-up. This can avoid turning a front mechanism around.

03 | BACKWARD TRAVEL CHANGES THE FACING CHOICE

Calculate the target bearing as usual. For forward travel, desired facing is the bearing. For backward travel, desired facing is bearing +180°, normalized.

The distance magnitude remains non-negative. The base command becomes negative for reverse travel.

Heading correction keeps the same rotational sign: left = base + turn, right = base - turn. Do not multiply steering by the reverse direction.

04 | TRACE A REVERSE CORRECTION

The robot faces 5° while retreating toward a target directly field-down. Desired reverse-facing heading is 0°, so the shortest error is -5°.

With base -15% and turn -3.5%, left output is -18.5% and right output is -11.5%. The left side moves backward faster, producing the needed anticlockwise correction.

If the reverse heading is poorly aligned, the example stops translation and turns in place first. Confirm rear clearance and the swept space during alignment.

05 | KEEP THE CURRENT POSE THROUGH THE ROUTE

Initialize the estimator once at the verified physical start. Continue updating it during turns, approaches and settling. Do not reset it to each intended waypoint.

A target describes where the robot should go. It is not an independent measurement that the robot arrived there. Resetting to it would hide arrival error and corrupt later geometry.

The example carries one pose across the entire route, checks each return status and stops before later waypoints if a phase fails.

06 | DESCRIBE WAYPOINTS WITH NAMED FIELDS

ROUTE = [
    make_waypoint("approach", 0, 600, final_heading=0),
    make_waypoint("retreat", 0, 200, reverse=True,
                  final_heading=0, drive_cap=12),
    make_waypoint("side position", 300, 200, final_heading=90)
]

A name helps logs identify where a route failed. The position uses the same field frame and reference as odometry. None for final heading means orientation on arrival is not specified.

The runner validates every waypoint before any route movement. It rejects non-finite coordinates, ambiguous direction values and invalid speed caps or deadlines.

07 | PLAN SEGMENT AND ROUTE DEADLINES

The example gives each waypoint a shared deadline for its alignment, drive and optional final turn. It also enforces a total route deadline.

The effective deadline is whichever comes first. Starting another phase does not grant a fresh full waypoint budget.

The 12-second route budget is a teaching example. Plan time for the actual task, mechanisms and operating period; do not assume a waypoint list will complete simply because each target is reachable.

A timeout stops the sequence. Do not skip ahead from an unknown physical endpoint.

08 | COMPLETE FORWARD / REVERSE ROUTE STARTER

This extends 07.2’s complete two-tracker navigator. Ports remain left drive 3/4, right drive 1/2, forward tracker 17, sideways tracker 19 and inertial 20.

Paste below # Begin project code, with generated VEX imports and Brain above it. Avoid duplicate Devices declarations. Configure geometry, directions, start pose and clearance before setting MOTION_CONFIGURED true.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER] | FORWARD / REVERSE WAYPOINT 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

def navigation_output(mode, pose, target_x, target_y, final_heading=None,
                      reverse=False, drive_cap=MAX_DRIVE):
    x, y, heading = pose
    distance, bearing = target_geometry(x, y, target_x, target_y)
    if mode == "HEADING":
        desired = final_heading % 360
        error = angle_error(desired, heading)
        near = abs(error) <= HEADING_TOL_DEG
    elif mode == "FACE":
        if distance <= POSITION_TOL_MM:
            return 0.0, 0.0, True  # At the point: no meaningful bearing needed.
        desired = (bearing + (180 if reverse else 0)) % 360
        error = angle_error(desired, heading)
        near = abs(error) <= HEADING_TOL_DEG
    else:  # DRIVE: explicit forward/reverse facing choice.
        if distance <= POSITION_TOL_MM:
            return 0.0, 0.0, True
        desired = (bearing + (180 if reverse else 0)) % 360
        error = angle_error(desired, heading)
        near = False
    if near:
        return 0.0, 0.0, True
    turn = clamp(TURN_KP * error, -MAX_TURN, MAX_TURN)
    base = 0.0
    if mode == "DRIVE" and abs(error) <= DRIVE_ALIGN_DEG:
        base = clamp(DISTANCE_KP * distance, 0, drive_cap)
        if reverse:
            base = -base
    left, right = base + turn, base - turn
    peak = max(abs(left), abs(right))
    if peak > MAX_WHEEL_OUTPUT:
        scale = MAX_WHEEL_OUTPUT / peak
        left *= scale
        right *= scale
    return left, right, False


# =============================================================================
# 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

# =============================================================================
# BOUNDED MOVEMENT LOOP
# =============================================================================
def run_navigation(mode, target_x, target_y, final_heading=None, timeout_ms=5000,
                   reverse=False, drive_cap=MAX_DRIVE, deadline_ms=None):
    stop_drive()
    if (mode not in ("FACE", "DRIVE", "HEADING") or timeout_ms <= 0
            or not all(math.isfinite(v) for v in (target_x, target_y))
            or (mode == "HEADING" and
                (final_heading is None or not math.isfinite(final_heading)))):
        return "INVALID_TARGET"
    started = previous_ms = brain.timer.time(MSEC)
    previous_pose = None
    settled_since = None
    try:
        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 deadline_ms is not None and now >= deadline_ms:
                return "DEADLINE"
            if now - started >= timeout_ms:
                return "TIMEOUT"
            current_pose = read_pose() # Faults latch pose invalid.
            left, right, near = navigation_output(
                mode, current_pose, target_x, target_y, final_heading, reverse, drive_cap)
            dt = (now - previous_ms) / 1000.0
            slow = False
            if previous_pose is not None and dt > 0:
                dx = current_pose[0] - previous_pose[0]
                dy = current_pose[1] - previous_pose[1]
                speed = math.sqrt(dx * dx + dy * dy) / dt
                rate = abs(angle_error(current_pose[2], previous_pose[2])) / dt
                slow = speed <= POSITION_RATE_TOL and rate <= TURN_RATE_TOL
            previous_pose, previous_ms = current_pose, 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
                side_output(left_drive, left)
                side_output(right_drive, right)
            wait(POLL_MS, MSEC)
    except Exception:
        return "POSE_OR_COMMAND_FAULT"
    finally:
        stop_drive()

# =============================================================================
# ROUTE DATA AND VALIDATION
# =============================================================================
def make_waypoint(name, x_mm, y_mm, reverse=False, final_heading=None,
                  drive_cap=15.0, timeout_ms=5000):
    if not isinstance(name, str) or not name:
        raise ValueError("Give the waypoint a name")
    if not isinstance(reverse, bool):
        raise ValueError("reverse must be True or False")
    if not all(isinstance(v, (int, float)) and math.isfinite(v)
               for v in (x_mm, y_mm, drive_cap, timeout_ms)):
        raise ValueError("Use finite numerical waypoint values")
    if not 0 < drive_cap <= MAX_DRIVE or timeout_ms <= 0:
        raise ValueError("Invalid drive cap or waypoint deadline")
    if final_heading is not None:
        if not isinstance(final_heading, (int, float)) or not math.isfinite(final_heading):
            raise ValueError("Invalid final heading")
        final_heading %= 360
    return {"name": name, "x_mm": x_mm, "y_mm": y_mm,
            "reverse": reverse, "final_heading": final_heading,
            "drive_cap": drive_cap, "timeout_ms": timeout_ms}

ROUTE_TIMEOUT_MS = 12000  # Example total route budget; plan for your task.
ROUTE = [
    make_waypoint("approach", 0, 600, final_heading=0),
    make_waypoint("retreat", 0, 200, reverse=True, final_heading=0, drive_cap=12),
    make_waypoint("side position", 300, 200, final_heading=90)
]

def validate_route(route):
    if not isinstance(route, (list, tuple)) or not route:
        raise ValueError("Provide a non-empty waypoint sequence")
    checked = []
    for w in route:
        checked.append(make_waypoint(w["name"], w["x_mm"], w["y_mm"],
                                     w["reverse"], w["final_heading"],
                                     w["drive_cap"], w["timeout_ms"]))
    return checked

# =============================================================================
# STOP-AND-SETTLE ROUTE RUNNER; NO POSE RESETS BETWEEN WAYPOINTS
# =============================================================================
def run_route(route):
    stop_drive()
    try:
        checked = validate_route(route)  # Check every waypoint before movement.
        if not math.isfinite(ROUTE_TIMEOUT_MS) or ROUTE_TIMEOUT_MS <= 0:
            return "INVALID_ROUTE_BUDGET"
        route_deadline = brain.timer.time(MSEC) + ROUTE_TIMEOUT_MS
        for w in checked:
            segment_deadline = min(route_deadline,
                                   brain.timer.time(MSEC) + w["timeout_ms"])
            phases = ["FACE", "DRIVE"]
            if w["final_heading"] is not None:
                phases.append("HEADING")
            for phase in phases:
                now = brain.timer.time(MSEC)
                if now >= segment_deadline:
                    return w["name"] + ": DEADLINE"
                result = run_navigation(
                    phase, w["x_mm"], w["y_mm"], w["final_heading"],
                    timeout_ms=segment_deadline-now, reverse=w["reverse"],
                    drive_cap=w["drive_cap"], deadline_ms=segment_deadline)
                print(w["name"], phase, result)
                if result != "SETTLED":
                    return w["name"] + ": " + result
            # Final rotation can shift the reference; verify position again.
            if brain.timer.time(MSEC) >= segment_deadline:
                return w["name"] + ": DEADLINE"
            current = read_pose()
            distance, bearing = target_geometry(current[0], current[1],
                                               w["x_mm"], w["y_mm"])
            if distance > POSITION_TOL_MM:
                return w["name"] + ": POSITION_CHANGED_AFTER_TURN"
            if (w["final_heading"] is not None and
                    abs(angle_error(w["final_heading"], current[2])) > HEADING_TOL_DEG):
                return w["name"] + ": FINAL_HEADING_NOT_CONFIRMED"
        return "ROUTE_COMPLETE"
    except Exception:
        return "ROUTE_OR_POSE_FAULT"
    finally:
        stop_drive()

def autonomous():
    global pose_valid
    stop_drive()
    if not ready or not MOTION_CONFIGURED:
        return
    try:
        initialize_pose()  # Physically verified START_POSE, once per run.
        print(run_route(ROUTE))
    except Exception:
        print("STARTUP_POSE_FAULT")
    finally:
        stop_drive()
        pose_valid = False

def driver_control():
    global pose_valid
    stop_drive() # Autonomous teaching project: no driver movement.
    pose_valid = False

competition = Competition(driver_control, autonomous)
stop_drive()
if MOTION_CONFIGURED and not competition.is_enabled() and inertial.installed():
    # Stationary disabled setup. Calibration may block.
    inertial.calibrate()
    ready = not inertial.is_calibrating() and not competition.is_enabled()
while True:
    wait(100, MSEC)

One callback owns pose updates and motor output. Do not add long blocking waits between phases without maintaining estimator updates.

Final rotation can shift the tracked reference. The runner checks position again after that turn and aborts if it moved outside tolerance, rather than reporting a complete waypoint.

Calibration may block during stationary disabled setup. Route deadlines supervise movement, not startup calibration. Driver mode commands no movement in this teaching project.

09 | ADD ACTIONS WITHOUT HIDING MOVEMENT FAILURE

A scoring or intake action should begin only after the waypoint’s movement and orientation checks pass. Keep action completion and failure separate from navigation status.

The starter contains no mechanism actions. When adding them, continue pose updates during any delay and use bounded action logic.

Do not treat a logged SETTLED as proof of physical scoring. Check the actual mechanism and field result with appropriate feedback or validation.

10 | CHOOSE WAYPOINTS FOR CLEARANCE

Waypoints divide a route into direct approaches. They do not create obstacle avoidance, guarantee a line stays clear or constrain the robot exactly to the drawn segment.

The navigator recomputes bearing from its current estimate and may steer away from the nominal straight line after drift. Allow a tested corridor for the entire footprint, including mechanisms.

Check reverse movement for rear obstacles and field objects. Also check the turn-in-place needed when changing travel direction or moving to a differently oriented segment.

11 | TEST SEGMENTS BEFORE THE FULL ROUTE

Log waypoint name, phase, direction, cap, elapsed time and return status. Keep estimator error separate from physical arrival error.

A successful first run does not establish repeatability. Test normal load, surface and mechanism positions.

12 | UNDERSTAND THE STOP-AND-SETTLE TRADEOFF

Stopping at every waypoint makes the route easy to inspect and reduces ambiguity at direction changes. It also adds settling time and can make a many-point route slow.

Adding more waypoints does not automatically make this point-chasing controller smooth. It can create repeated alignment and braking.

Use a smaller number of purposeful waypoints for this lesson. Continuous lookahead and curvature control belong to the next stage, with different waypoint-completion rules.

Ordinary failures stop through finally. Forced task termination needs deliberate mode-transition testing and a single drivetrain output owner.

CHALLENGES | BUILD A MIXED-DIRECTION ROUTE

1. Draw: mark a three-waypoint route with incoming travel directions and final headings.
2. Calculate: find backward-facing headings in all four quadrants.
3. Trace: predict left/right output for a reverse heading correction.
4. Validate: reject a malformed direction or speed cap before movement.
5. Test: verify forward and reverse segments separately.
6. Bound: demonstrate a shared waypoint deadline across phases.
7. Abort: simulate failure at the middle waypoint and verify later phases do not run.
8. Evaluate: explain why many stop-and-settle points differ from continuous path following.

OFFICIAL REFERENCES

• VEX competition callback and mode API
• VEX motor output API

The waypoint runner, route data and control policy are independently written teaching examples. VEX and VEXcode are trademarks of their respective owners.