01 | FROM GEOMETRY TO MOVEMENT

In 07.1 you calculated the distance and bearing from an estimated position to a target. Now use those values repeatedly to control movement.

A simple sequence is: turn toward the point, drive toward it while correcting heading, then optionally turn to a final pose heading.

The fixed target does not imply a fixed bearing. As the robot moves or drifts, its direction to the target changes. Recalculate using current pose feedback.

02 | REQUIRE A VALID FIELD POSE

This starter uses two perpendicular tracking wheels and inertial rotation with the field integration from 06.5. It controls the same reference point that odometry estimates.

Start at a verified physical pose, with calibrated wheel scales, signed offsets and correct sensor directions. A numerical start pose cannot confirm the robot’s placement.

A feedback fault or excessive update gap invalidates the estimate and stops the movement. Do not substitute the last good pose as if it were current.

03 | TURN TOWARD THE CURRENT BEARING

Subtract the robot position from the target, then calculate bearing with atan2(dx, dy). Compare bearing with current heading using a shortest signed error.

At (0, 0), target (500, 500) lies 707.1 mm away at bearing 45°. A robot facing zero initially needs a clockwise correction.

Positive turn output sends the left side forward and right side backward. The example turns in place until the heading error is within tolerance and movement settles.

If already within the point’s position window, turning toward that point has no useful unique direction. The function stops and settles rather than inventing a bearing.

04 | DRIVE ONLY WHEN REASONABLY ALIGNED

Distance error supplies a forward request. Heading error supplies steering. The starter allows translation only while the target is within a 12-degree alignment window.

Outside that window, base output is zero and the robot rotates toward the target. This avoids driving directly away from a target behind it.

The hard alignment threshold is a simple teaching policy. It can switch between turn and drive near the boundary. Later designs can use hysteresis or smooth speed reduction.

05 | MIX AND LIMIT WHEEL OUTPUT

As in heading hold, left = base + turn and right = base - turn. Scale both together if either exceeds the wheel-output cap.

The starter caps forward request at 20%, turning at 12% and wheel output at 25%. These values require physical tuning.

Close to the target or during alignment, one side may reverse. Check clearance for the robot’s full footprint and mechanisms, including a turn in place.

This controller has P terms only. It has no integral memory, obstacle detection or automatic collision avoidance.

06 | FINISH WITH POSITION AND SETTLING CHECKS

Drive-to-point success requires the estimated reference point to be within 20 mm of the target, estimated translation speed below 30 mm/s and turn rate below 8°/s for 200 ms.

The controller brakes inside the position window. If the robot leaves it before settling, movement resumes.

FACE and HEADING modes require their angular tolerance and the same low-motion settling checks. A final heading is a separate command after reaching the point.

A rotation about an imperfect reference can change X/Y after the position step. For a strict final pose, verify position again after the final turn or use a coupled pose controller.

07 | COMPLETE FORWARD DRIVE-TO-POINT STARTER

Paste below # Begin project code, keeping generated VEX imports and Brain setup above it. The four motors use left ports 3/4 and right 1/2; trackers use 17/19 and inertial uses 20. Avoid duplicate Devices declarations.

MOTION_CONFIGURED begins false. Verify signs, scales, offsets, physical start and a clear test area before enabling movement.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER] | FORWARD DRIVE-TO-POINT 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):
    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 = clamp(DISTANCE_KP * distance, 0, MAX_DRIVE)
    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):
    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 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()

def turn_toward_target(x_mm, y_mm, timeout_ms=3000):
    return run_navigation("FACE", x_mm, y_mm, timeout_ms=timeout_ms)

def drive_to_point(x_mm, y_mm, timeout_ms=5000):
    return run_navigation("DRIVE", x_mm, y_mm, timeout_ms=timeout_ms)

def turn_to_field_heading(heading_deg, timeout_ms=3000):
    return run_navigation("HEADING", 0, 0, heading_deg, timeout_ms)

# =============================================================================
# COMPETITION CALLBACKS
# =============================================================================
def autonomous():
    global pose_valid
    stop_drive()
    if not ready or not MOTION_CONFIGURED:
        return
    try:
        initialize_pose() # Robot must actually be at START_POSE.
        result = turn_toward_target(500, 500)
        print("face", result)
        if result != "SETTLED":
            return
        result = drive_to_point(500, 500)
        print("drive", result)
        if result != "SETTLED":
            return
        print("final heading", turn_to_field_heading(90))
    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 autonomous callback owns both odometry updates and motor output. It continually updates pose during every movement loop, including turning and settling. There is no background odometry thread to conflict with.

The example does not insert long blocking pauses between commands. If you add waits, keep updating odometry during them or use a carefully designed dedicated estimator owner; otherwise the gap check will invalidate the estimate.

Autonomous reinitializes only because the robot is assumed physically placed at START_POSE for that run. Do not restart the routine midway across the field without a verified new start.

08 | USE RETURN STATUS TO CONTROL THE SEQUENCE

Timeouts apply separately to each action. Three commands can therefore consume more time than one command’s deadline. Plan the total route budget separately.

Normal returns and exceptions stop through finally. Forced task termination can bypass ordinary cleanup, so test competition mode transitions and maintain one motor-output owner.

09 | POINT ARRIVAL AND FINAL HEADING ARE SEPARATE

Driving to (500, 500) does not imply facing 90° on arrival. The target bearing depends on the approach, while final heading describes the desired orientation.

# Inside the configured autonomous routine:
result = drive_to_point(500, 500)
if result == "SETTLED":
    result = turn_to_field_heading(90)

All three functions use the estimator’s field heading, not an unrelated raw inertial zero. This keeps the route frame consistent.

Reverse segments and waypoint sequencing are covered next. Do not simply negate this radial distance request and expect a correct backwards controller.

10 | TEST SHORT, CLEAR ROUTES FIRST

1. Validate odometry physically before enabling point control.
2. Test turn-toward-target at a short, measured target in each quadrant.
3. Drive to a target directly ahead at a conservative cap.
4. Test an initially misaligned target and observe rotate-only behaviour.
5. Repeat three approaches from the same start.
6. Measure the actual endpoint before comparing it with the estimate.
7. Test fault and cancellation branches in simulation, then controlled mode changes.

Do not assume a direct segment is clear because its endpoint is reachable. Check obstacles, robot footprint and the swept space during turns.

11 | RECOGNIZE THE LIMITATIONS

Radial point chasing is a basic navigator, not a path planner. It can stop and rotate repeatedly when drift or overshoot changes the bearing.

Near an overshot target, bearing can flip by about 180°. The position window and reduced output help, but poor tuning can still cause repeated re-approaches until timeout.

Wheel slip or heading drift can cause a reported success at the wrong physical position. The example has no obstacle sensor or dedicated stall detector.

Tune turn behaviour first, then distance gain and caps. Keep physical arrival error and estimator error separate in your log.

CHALLENGES | FACE, DRIVE AND VERIFY

1. Calculate: find bearing and distance to points in all four quadrants.
2. Trace: predict output when the target is behind the robot.
3. Simulate: compare rotate-only and aligned-drive requests.
4. Verify: test three arrivals at a measured point.
5. Orient: finish at a heading different from the approach bearing.
6. Cancel: confirm B and mode changes stop the routine.
7. Fault: verify invalid pose data cannot produce a successful arrival.
8. Evaluate: explain when a waypoint path is preferable to direct point chasing.

OFFICIAL REFERENCES

• VEX competition callback and mode API
• VEX motor commands
• VEX inertial rotation and calibration API

The navigator and control policy are independently written teaching examples. VEX and VEXcode are trademarks of their respective owners.