01 | DEFINE A COMPLETE GOAL

A delivery robot can reach a shelf but face away from it. A VEX robot can arrive at a goal yet leave its intake pointing in the wrong direction. Position alone is not enough.

A target pose contains X, Y and heading. In this guide, (500 mm, 500 mm, 90°) means place the robot reference point near that field coordinate, then face field +X.

Use the earlier field convention: +X right, +Y up the field, clockwise headings from +Y. Distances refer to the odometry reference point, ideally the drivetrain centre.

02 | APPROACH BEARING IS NOT FINAL HEADING

From (0, 0), the point (500, 500) lies at bearing 45°. That is a useful initial travel direction. The requested final heading of 90° is a separate orientation.

If you turn to 90° immediately and drive straight forward, you travel east rather than toward the point. First reach the position, then perform the final rotation.

This lesson uses a clear three-stage policy: FACE → DRIVE → HEADING. Each stage needs feedback and a settled finish. A failed stage ends the sequence.

03 | SLOW BEFORE THE POSITION WINDOW

At full approach speed, the robot may carry momentum through the target. Braking only after crossing a tolerance boundary can be too late.

The starter combines a distance-proportional request with a decreasing upper limit. Outside a 600 mm slow zone, the upper limit is 20%. It falls linearly toward 4% at the 20 mm position-window edge.

The actual request is the smaller of that envelope and 0.05 × distance. Once inside the position window, the controller requests a brake stop and observes settling.

The near-goal cap is not a minimum speed. The example does not force a 4% request near the goal. A minimum-speed floor can overcome friction, but can also cause repeated overshoot.

# =============================================================================
# FINAL APPROACH SETTINGS AND REQUEST ENVELOPE
# =============================================================================
SLOW_ZONE_MM = 600.0
NEAR_GOAL_CAP = 4.0       # Upper bound at the position-window edge, not a floor
GOAL_TIMEOUT_MS = 14000  # Shared by FACE, DRIVE, HEADING and final verification
GOAL = (500.0, 500.0, 90.0)

def approach_request(distance_mm):
    if not math.isfinite(distance_mm) or distance_mm < 0:
        raise ValueError("Invalid distance")
    if distance_mm <= POSITION_TOL_MM:
        return 0.0
    fraction = clamp((distance_mm-POSITION_TOL_MM) /
                     (SLOW_ZONE_MM-POSITION_TOL_MM), 0.0, 1.0)
    envelope = NEAR_GOAL_CAP + (MAX_DRIVE-NEAR_GOAL_CAP)*fraction
    return min(DISTANCE_KP*distance_mm, envelope)

The complete starter below contains this function and its settings. The envelope limits the centre forward request; steering can make an individual wheel request larger. Both wheel outputs still share a 25% cap.

04 | A LOWER REQUEST DOES NOT GUARANTEE A STOP

Motor velocity percentages do not directly tell you the robot’s ground speed in mm/s. Wheel size, gearing, battery, load and slip all affect the physical result.

For ideal constant deceleration, stopping distance is v² / (2a). At v = 300 mm/s and a = 600 mm/s², the ideal distance is 75 mm. Doubling speed to 600 mm/s increases it to 300 mm.

A 20 ms sensing and command delay adds about 6 mm of travel at 300 mm/s before deceleration begins. Real braking can differ further. Use measured stopping trials and a margin when selecting the slow zone.

This program uses a request envelope, not a closed-loop physical deceleration controller. For a model-based speed cap such as √(2a × remaining distance), first define consistent units, validated deceleration and a calibrated velocity controller.

05 | SETTLE BEFORE CALLING IT ARRIVAL

Passing through the position window at speed is not a successful stop. The DRIVE phase brakes in the window and checks that estimated motion stays low for 200 ms.

If the robot leaves the position window, the interval resets and the drive phase resumes. If it cannot settle before the deadline, the routine fails.

Low requests can stall against static friction. Investigate gain, calibration and the tolerance required by the mechanism before adding a minimum-speed floor. Do not label a timeout as success.

06 | TURN USING THE SHORTEST HEADING ERROR

After position arrival, turn in place toward the requested field heading. Use the same field frame as the estimator, rather than an unrelated inertial zero.

error_deg = (target_heading - current_heading + 180) % 360 - 180

From heading 350° to 10°, the error is +20°, so the short turn is clockwise. From 10° to 350°, it is −20°, so turn anticlockwise.

The heading phase uses a capped proportional request. It brakes within 2° of the target and checks the same low-motion settling interval. At exactly 180° either direction is equally short; this formula chooses −180°.

As you rotate, the chassis and mechanisms sweep through space. A clear reference-point location does not guarantee clearance for the full robot.

07 | CHECK POSITION AGAIN AFTER TURNING

An ideal differential drive rotates around its centre. A real robot can scrub, slip or move asymmetrically. An offset reference point can also move during rotation.

The starter reads a fresh pose after the heading phase settles. It checks both position and heading again before returning POSE_SETTLED.

If the final turn moved the reference point outside 20 mm, it returns POSITION_CHANGED_DURING_TURN. It does not automatically drive back and repeat the turn indefinitely.

Repeatedly correcting position and heading can form a loop that never settles. A more advanced recovery needs a limited retry count, a shared deadline and clear evidence that another correction is useful.

08 | COMPLETE FINAL-POSE STARTER

Paste below # Begin project code, outside the generated region. Keep generated VEX imports and Brain setup above it. Avoid duplicate Devices declarations.

Four drive motors use left ports 3/4 and right 1/2. The forward tracker is on 17, sideways tracker on 19 and inertial on 20. All directions, scales and offsets are examples to verify.

Start physically at START_POSE. MOTION_CONFIGURED begins False. Enable it only after checking configuration, odometry and a clear short test route.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER] | FINAL POSITION AND HEADING 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

# =============================================================================
# FINAL APPROACH SETTINGS AND REQUEST ENVELOPE
# =============================================================================
SLOW_ZONE_MM = 600.0
NEAR_GOAL_CAP = 4.0       # Upper bound at the position-window edge, not a floor
GOAL_TIMEOUT_MS = 14000  # Shared by FACE, DRIVE, HEADING and final verification
GOAL = (500.0, 500.0, 90.0)

def approach_request(distance_mm):
    if not math.isfinite(distance_mm) or distance_mm < 0:
        raise ValueError("Invalid distance")
    if distance_mm <= POSITION_TOL_MM:
        return 0.0
    fraction = clamp((distance_mm-POSITION_TOL_MM) /
                     (SLOW_ZONE_MM-POSITION_TOL_MM), 0.0, 1.0)
    envelope = NEAR_GOAL_CAP + (MAX_DRIVE-NEAR_GOAL_CAP)*fraction
    return min(DISTANCE_KP*distance_mm, envelope)

def navigation_output(mode, pose, target_x, target_y, final_heading=None):
    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.
        error = angle_error(bearing, heading)
        near = abs(error) <= HEADING_TOL_DEG
    else:  # DRIVE, forward-only in this lesson.
        if distance <= POSITION_TOL_MM:
            return 0.0, 0.0, True
        error = angle_error(bearing, 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 = approach_request(distance)
    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,
                   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)
            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()

# =============================================================================
# SHARED-DEADLINE FINAL POSE SEQUENCE
# =============================================================================
def finish_at_pose(target_x, target_y, final_heading, timeout_ms=GOAL_TIMEOUT_MS):
    stop_drive()
    try:
        values = (target_x, target_y, final_heading, timeout_ms,
                  SLOW_ZONE_MM, NEAR_GOAL_CAP, MAX_DRIVE, DISTANCE_KP)
        if (not all(isinstance(v, (int, float)) and math.isfinite(v) for v in values)
                or timeout_ms <= 0 or SLOW_ZONE_MM <= POSITION_TOL_MM
                or not 0 < NEAR_GOAL_CAP <= MAX_DRIVE <= MAX_WHEEL_OUTPUT
                or DISTANCE_KP <= 0):
            return "INVALID_GOAL_OR_CONFIGURATION"
        final_heading %= 360
        deadline = brain.timer.time(MSEC) + timeout_ms
        for phase in ("FACE", "DRIVE", "HEADING"):
            remaining = deadline-brain.timer.time(MSEC)
            if remaining <= 0:
                return "DEADLINE"
            result = run_navigation(phase, target_x, target_y, final_heading,
                                    timeout_ms=remaining, deadline_ms=deadline)
            print(phase, result)
            if result != "SETTLED":
                return phase + ": " + result
        # The turn has settled. Check position again with fresh feedback.
        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"
        if brain.timer.time(MSEC) >= deadline:
            return "DEADLINE"
        current = read_pose()
        distance, bearing = target_geometry(current[0], current[1], target_x, target_y)
        if distance > POSITION_TOL_MM:
            return "POSITION_CHANGED_DURING_TURN"
        if abs(angle_error(final_heading, current[2])) > HEADING_TOL_DEG:
            return "HEADING_CHANGED_AFTER_SETTLE"
        return "POSE_SETTLED"
    except Exception:
        return "POSE_OR_COMMAND_FAULT"
    finally:
        stop_drive()

# =============================================================================
# COMPETITION CALLBACKS
# =============================================================================
def autonomous():
    global pose_valid
    stop_drive()
    if not ready or not MOTION_CONFIGURED:
        return
    try:
        initialize_pose()  # Physically placed at START_POSE before this run.
        print("final pose", finish_at_pose(*GOAL))
    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)

This is a standalone, forward point-approach example based on 07.2, with a final-pose finish policy. It is not a second path follower running alongside 07.4.

One callback owns pose updates and drivetrain output. Mode changes, B, stale feedback, failed settling or exceptions prevent the sequence from reporting success.

09 | SHARE ONE DEADLINE ACROSS THE GOAL

Giving each stage a fresh timeout can multiply the time spent on one goal. Instead, establish one absolute deadline before FACE starts.

Each stage receives only the remaining time. Final verification also checks that the deadline has not expired. The example budget is fourteen seconds; it is a configuration choice, not a guarantee of completion.

Reported success is about the estimate. Record measured physical arrival and heading separately to identify odometry error.

10 | CONNECT THE POLICY TO PURE PURSUIT

In 07.4, the path follower already brakes and settles near its endpoint. You can use that successful arrival as the handover to a final heading action.

1. Keep the path follower as the sole drive owner during path tracking.
2. Reduce its cruise request as the route approaches the final region.
3. Require a successful endpoint stop before starting the final rotation.
4. Continue updating pose throughout the turn.
5. Verify final position and heading, with one budget covering both actions.

For a long path that passes close to its endpoint early, radial endpoint distance alone can slow the robot too soon. Combine remaining path progress with endpoint distance to identify the final region.

To adapt the 07.4 code, its follower and final-turn routine must accept the same absolute deadline and share the same estimator. Do not paste two full standalone programs together or run competing output loops.

11 | TEST POSITION AND HEADING SEPARATELY

1. Test short straight arrivals with no final turn.
2. Measure arrival error for the same target at several cruise caps.
3. Add a small final heading change, then a larger one.
4. Measure physical position before and after rotation.
5. Test wraparound headings such as 350° to 10°.
6. Repeat the same full goal from the same physical start.
7. Test timeout, cancellation and invalid feedback in simulation.

Log the target, approach settings, returned status, estimated X/Y/heading, measured endpoint, measured orientation and total time. Tune one factor at a time.

This starter has no obstacle avoidance or dedicated stall detector. Brake mode cannot eliminate momentum, slip or every source of physical error.

CHALLENGES | ARRIVE, ORIENT AND VERIFY

1. Calculate: find the forward request at distances 600, 400, 200 and 100 mm using the starter settings.
2. Compare: explain why a 4% upper bound is different from a 4% minimum-speed floor.
3. Predict: work out heading errors for 350° → 10°, 10° → 350° and 90° → 270°.
4. Estimate: calculate ideal stopping distance at 250 mm/s with deceleration 500 mm/s². Add travel during a 40 ms delay.
5. Test: compare three physical arrivals with and without a final 90° rotation.
6. Fault: simulate a final rotation that moves X/Y outside the position window. Check the returned status.
7. Budget: show that a failed phase or expired deadline prevents later movement.
8. Evaluate: describe when a stop-and-turn finish is preferable to approaching with the desired heading already aligned.

OFFICIAL REFERENCES

• VEX motor velocity requests and stopping modes
• VEX competition callbacks and mode checks
• Nav2 regulated pure pursuit: approach and goal-heading options

The starter and worked examples are independently written teaching material. The program requires configuration, physical testing and tuning on your robot.