01 | DISTANCE AND DIRECTION ARE DIFFERENT

A robot can reach the expected encoder distance while drifting off its intended direction. Unequal friction, load and wheel behaviour can make an open-loop straight drive curve.

Heading hold compares the desired robot orientation with the inertial measurement and adds a steering correction while distance control supplies forward or reverse movement.

The distance loop asks how much movement remains. The heading loop asks which way the robot should face. They share the same motors, so combine their requests before sending output.

02 | CAPTURE THE TARGET ONCE

For a simple straight move, capture the current heading at the start and keep it fixed until the move ends. Alternatively, pass an existing route heading.

# Calls used inside the configured autonomous project:
# Capture the current heading once for this move.
result = drive_heading(500)

# Use the same previously measured heading for both directions.
route_heading = inertial.heading(DEGREES)
result = drive_heading(500, route_heading)
if result == "settled":
    result = drive_heading(-500, route_heading)

Reading a fresh target on every loop update would make target and measurement equal, removing the useful error.

An inertial heading is relative to its configured reference, not automatically a compass direction. Preserve that reference through the route. Setting the displayed heading does not physically turn the robot.

03 | CALCULATE THE SHORT HEADING ERROR

Heading error = the shortest signed turn from current heading to target. Use the wrap formula from earlier guides.

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

Heading hold makes small corrections. If the robot starts facing a very different direction, turn first rather than treating a straight-drive function as a combined turn-and-drive planner.

04 | COMBINE BASE AND STEERING

For forward-positive wheel commands and clockwise-positive heading error:

base = distance_kp * distance_error
correction = heading_kp * heading_error_value
left_output = base + correction
right_output = base - correction

A positive correction makes the left command more positive than the right, creating a clockwise turning tendency. A negative correction does the opposite.

05 | REVERSE WITHOUT REVERSING THE CORRECTION

Do not multiply heading correction by the travel direction. A negative base handles reversing. The steering difference still determines the robot’s rotation.

With base -20% and correction +3%, the left side reverses at 17% and the right at 23%. The right side moves backward faster, so the robot rotates clockwise.

Holding heading while reversing means the robot keeps facing the same direction while travelling backward. It does not turn to face the direction of travel.

If reverse correction increases heading error, verify motor signs, sensor orientation and the mixing formula before increasing the gain.

06 | LIMIT THE COMBINED COMMAND

Bound distance output and steering correction separately, then limit their combined wheel commands.

For example, base +25% and correction +8% request left +33% and right +17%. A common scale of 25/33 gives approximately +25% and +12.9%.

Scaling both by the same factor preserves the ratio of the mixed commands. Clipping each side independently changes that ratio. Neither approach guarantees a particular physical turn under changing traction.

def mix_outputs(base, correction, limit):
    left = base + correction
    right = base - correction
    peak = max(abs(left), abs(right))
    if peak > limit:
        scale = limit / peak
        left *= scale
        right *= scale
    return left, right

The starter uses P only in both loops, so it has no integral state to wind up. If you add I later, account for the final mixed and limited output, as discussed in 05.3.

07 | CHOOSE THE FINISH AND ABORT POLICIES

The starter finishes only when distance and heading are within their windows, their estimated rates are low, and those conditions persist for 200 ms.

Inside both position windows, it brakes and observes settling. Near the distance target but outside the heading window, it sets base to zero and makes a small heading correction. This can pivot or briefly reverse one side.

It aborts after excessive heading deviation, timeout, invalid feedback, a long loop gap, Controller B or a mode/readiness change.

The 25-degree deviation threshold is an example. Choose it for a tested route and clearance. A controller cannot know that a correction path is free of obstacles.

08 | CONFIGURE AND VERIFY THE ROBOT

1. Confirm left ports 3/4 and right ports 1/2, with the listed blue cartridges.
2. Verify both forward commands produce forward wheel movement and increasing encoder values.
3. Enter effective wheel diameter and external wheel-turn ratio.
4. Mount the inertial sensor securely on Smart Port 20 and check its heading convention.
5. Calibrate while stationary and disabled before enabling autonomous.
6. Test with low caps, short distances and a clear floor area.

Set MOTION_CONFIGURED only after these checks. Reversal flags depend on the actual build. Keep generated imports and Brain setup above this manually configured example, and avoid duplicate Devices declarations.

09 | COMPLETE AUTONOMOUS HEADING-HOLD STARTER

Paste below # Begin project code, outside the generated configuration. The example moves forward 500 mm and reverses 500 mm using one captured route heading. Driver mode commands no movement.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER] | DISTANCE + HEADING HOLD STARTER
# =============================================================================
# Keep generated imports and brain above '# Begin project code'.
# Manual configuration below: do not duplicate these devices in Devices.
# Left motors: ports 3 and 4; right motors: ports 1 and 2.
# Blue cartridges. Reversal settings must match your physical robot.
# Inertial: Smart Port 20. Controller B cancels movement.
import math

left_motor_1 = Motor(Ports.PORT3, GearSetting.RATIO_6_1, False)
left_motor_2 = Motor(Ports.PORT4, GearSetting.RATIO_6_1, False)
right_motor_1 = Motor(Ports.PORT1, GearSetting.RATIO_6_1, True)
right_motor_2 = Motor(Ports.PORT2, GearSetting.RATIO_6_1, True)
left_drive = MotorGroup(left_motor_1, left_motor_2)
right_drive = MotorGroup(right_motor_1, right_motor_2)
inertial = Inertial(Ports.PORT20)
controller = Controller(PRIMARY)

# =============================================================================
# MEASURED GEOMETRY AND STARTER GAINS (EXAMPLES, NOT TUNED VALUES)
# =============================================================================
MOTION_CONFIGURED = False       # Enable after direction/geometry checks
WHEEL_DIAMETER_MM = 100.0       # Replace with effective measured diameter
WHEEL_TURNS_PER_MOTOR_TURN = 1.0 # Direct drive = 1; external gearing changes it
DRIVE_KP = 0.08                 # Percent output per mm of error
HEADING_KP = 0.7                # Percent output per degree
MAX_CORRECTION = 8.0            # Bound steering correction
MAX_HEADING_ERROR = 25.0        # Abort excessive deviation
DISTANCE_RATE_TOL = 30.0        # mm/s for settling
HEADING_RATE_TOL = 8.0          # degrees/s for settling
MAX_LOOP_GAP_S = 0.10
MAX_OUTPUT = 25.0               # Conservative test cap
DRIVE_TOL_MM = 10.0
TURN_TOL_DEG = 2.0
SETTLE_MS = 200
LOOP_MS = 20
ready = False

# =============================================================================
# SHARED OUTPUT AND FEEDBACK
# =============================================================================
def clamp(value, low, high):
    return max(low, min(high, value))

def stop_drive():
    left_drive.stop(BRAKE)
    right_drive.stop(BRAKE)

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

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

def side_mm(motor_a, motor_b):
    turns = (motor_a.position(TURNS) + motor_b.position(TURNS)) / 2
    return turns * math.pi * WHEEL_DIAMETER_MM * WHEEL_TURNS_PER_MOTOR_TURN

def permitted():
    return (ready and MOTION_CONFIGURED and competition.is_enabled()
            and competition.is_autonomous()
            and not controller.buttonB.pressing()
            and inertial.installed() and not inertial.is_calibrating())

# =============================================================================
# COMBINE TRANSLATION AND CLOCKWISE STEERING
# =============================================================================
def mix_outputs(base, correction, limit):
    left = base + correction
    right = base - correction
    peak = max(abs(left), abs(right))
    if peak > limit:
        scale = limit / peak
        left *= scale
        right *= scale
    return left, right

def average_mm():
    return (side_mm(left_motor_1, left_motor_2)
            + side_mm(right_motor_1, right_motor_2)) / 2.0

# =============================================================================
# REUSABLE STRAIGHT MOVE: POSITIVE FORWARD, NEGATIVE REVERSE
# heading_deg=None captures the heading once, at the start of this move.
# =============================================================================
def drive_heading(distance_mm, heading_deg=None, timeout_ms=4000):
    stop_drive()
    if not permitted():
        return "cancelled or not ready"
    if (not math.isfinite(distance_mm) or timeout_ms <= 0
            or DRIVE_KP <= 0 or HEADING_KP <= 0
            or MAX_OUTPUT <= 0 or MAX_CORRECTION < 0):
        return "invalid configuration"
    try:
        current_heading = inertial.heading(DEGREES)
        target_heading = (current_heading if heading_deg is None
                          else heading_deg) % 360
        start_distance = average_mm()
        if not all(math.isfinite(v) for v in
                   (current_heading, target_heading, start_distance)):
            return "invalid feedback"
        started = brain.timer.time(MSEC)
        previous_time = started
        previous_distance = None
        previous_heading = None
        settled_since = None
        while True:
            if not permitted():
                return "cancelled or not ready"
            now = brain.timer.time(MSEC)
            if now - started >= timeout_ms:
                return "timeout"
            dt = (now - previous_time) / 1000.0
            if dt <= 0:
                wait(LOOP_MS, MSEC)
                continue
            if dt > MAX_LOOP_GAP_S:
                return "loop timing fault"
            previous_time = now
            travelled = average_mm() - start_distance
            heading = inertial.heading(DEGREES)
            if not math.isfinite(travelled) or not math.isfinite(heading):
                return "invalid feedback"
            distance_error = distance_mm - travelled
            h_error = heading_error(target_heading, heading)
            if abs(h_error) > MAX_HEADING_ERROR:
                return "heading deviation"
            slow = False
            if previous_distance is not None:
                speed = (travelled - previous_distance) / dt
                turn_rate = heading_error(heading, previous_heading) / dt
                slow = (abs(speed) <= DISTANCE_RATE_TOL
                        and abs(turn_rate) <= HEADING_RATE_TOL)
            previous_distance = travelled
            previous_heading = heading
            near_distance = abs(distance_error) <= DRIVE_TOL_MM
            near_heading = abs(h_error) <= TURN_TOL_DEG
            if near_distance and near_heading:
                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
                base = (0.0 if near_distance else
                        clamp(DRIVE_KP * distance_error,
                              -MAX_OUTPUT, MAX_OUTPUT))
                correction = (0.0 if near_heading else
                              clamp(HEADING_KP * h_error,
                                    -MAX_CORRECTION, MAX_CORRECTION))
                left_u, right_u = mix_outputs(base, correction, MAX_OUTPUT)
                side_output(left_drive, left_u)
                side_output(right_drive, right_u)
            wait(LOOP_MS, MSEC)
    except Exception:
        return "feedback or command error"
    finally:
        stop_drive()

# =============================================================================
# COMPETITION ENTRY POINTS: ONE DRIVE OUTPUT OWNER
# =============================================================================
def autonomous():
    stop_drive()
    if not permitted():
        return
    # Capture once for the whole out-and-back pair.
    route_heading = inertial.heading(DEGREES)
    for distance in (500, -500):
        result = drive_heading(distance, route_heading)
        print("distance", distance, "result", result)
        if result != "settled":
            break
        wait(300, MSEC)
    stop_drive()

def driver_control():
    # Autonomous teaching project: no driver movement.
    stop_drive()

competition = Competition(driver_control, autonomous)
stop_drive()
# Calibrate while disabled and stationary. This call may block.
# Movement timeouts do not supervise startup calibration.
if MOTION_CONFIGURED and not competition.is_enabled() and inertial.installed():
    inertial.calibrate()
    ready = not inertial.is_calibrating() and not competition.is_enabled()
print("Heading hold ready:", ready)
while True:
    wait(100, MSEC)

Each distance is relative to the encoder snapshot at the start of its own call. The reverse call therefore requests -500 mm from the forward endpoint, rather than an absolute encoder position.

Distance feedback is the average of both sides. Differential wheel movement used for heading correction tends to cancel in that average. This is a centre-travel estimate, not proof of straight-line displacement on the field.

Keep one Competition object and one drivetrain output owner when integrating with your team project. A separate heading thread and distance thread must not fight over the motor commands.

Calibration may block before movement. Timeouts cover the movement loop, not calibration. Ordinary returns and exceptions brake through finally; forced task termination needs deliberate mode-transition testing.

10 | TEST FORWARD AND REVERSE SEPARATELY

Start aligned with the route heading. Compare three trials with heading correction disabled in a separate controlled test and three with it enabled.

Use recorded drift rather than pushing or twisting a moving robot by hand. Do not create fault tests near people, walls or field objects.

11 | TUNE AND UNDERSTAND THE LIMITS

Tune distance response first, then add a small heading gain and correction cap. If steering hunts from side to side, reduce the heading gain and inspect feedback noise or mechanical play.

A heading controller controls orientation, not lateral field position. After a sideways displacement it can face the correct direction while still following a parallel offset path. Returning to a field line needs position feedback and a path-control design.

Wheel slip, a biased encoder conversion or inertial drift can still produce a poor physical result. Average encoder travel can also hide abnormal wheel behaviour. The starter has no dedicated stall or per-wheel disagreement detector.

Large correction near the endpoint can pivot the robot. If your application forbids wheel reversal, choose a different mixing policy and document its reduced heading authority.

CHALLENGES | HOLD DIRECTION IN BOTH DIRECTIONS

1. Calculate: mix base +18% and correction -4%, then repeat with base -18%.
2. Explain: why does clockwise correction keep the same sign while reversing?
3. Scale: limit base +25% and correction +8% to a 25% wheel cap.
4. Wrap: calculate error for target 2 degrees and current 358 degrees.
5. Verify: document sensor and encoder signs before enabling movement.
6. Compare: record forward and reverse trials with the same route heading.
7. Abort: test excessive heading deviation in simulation.
8. Evaluate: explain why correct heading does not prove correct lateral position.

OFFICIAL REFERENCES

• VEX inertial heading and calibration API
• VEX motor output and encoder API

The heading controller, mixing policy and challenges are independently written learning examples. VEX and VEXcode are trademarks of their respective owners.