01 | TEST SMALL ACTIONS BEFORE A FULL ROUTE

A scoring routine combines movements, mechanisms and timing. When it fails, the cause can be difficult to identify. A test course separates the basic movements so each can be measured and improved.

Begin with four independent stations: straight, turn, arc and reverse. Once each is repeatable, combine them into a short sequence and observe how errors accumulate.

The goal is evidence: requested targets, sensor outcomes, physical measurements and the conditions under which the results were obtained.

02 | SET UP FOUR STATIONS

Use a level surface, tape marks, a ruler or tape measure and a fixed camera if available. Mark one consistent chassis reference point for position measurements.

Each independent station begins from a documented start position and orientation. Reset the robot physically between trials while disabled. Never reposition it by hand while enabled.

Allow clearance for the whole robot and its turning footprint. A small centre-path radius can still sweep a large corner of the chassis.

03 | PREDICT THE RESULTS FIRST

The arc station should change heading by path ÷ radius: 300 ÷ 400 = 0.75 radians, approximately 42.97° left.

With track width 300 mm, inside travel is 0.75 × 250 = 187.5 mm, while outside travel is 0.75 × 550 = 412.5 mm. The right side is outside for this forward left arc.

Starting at heading zero, the combined sequence ideally finishes at approximately 47.03° right of the original forward direction: right 90° followed by left 42.97°. Reverse travel should retain that heading.

The course does not return to its start. Lay out and check every segment, including reverse clearance, rather than assuming it is a closed loop.

04 | DEFINE PASS CRITERIA BEFORE TUNING

Set criteria that match your measurement method and intended task. A tape measurement and a camera estimate have uncertainty.

Separate physical pass criteria from software tolerances. An encoder-based success result does not automatically pass the physical distance or path test.

05 | KEEP CONDITIONS COMPARABLE

Record wheel type and size, gearing, track width, robot mass or payload, surface, battery state, speed settings and code version.

Keep the same starting orientation and reference marks. A poor start can create a route error that looks like a movement-controller problem.

Change one setting at a time. Save a working baseline before trying a new acceleration rate, stopping policy or profile.

06 | COMPLETE SELECTABLE TEST PROJECT

This project combines the monitored turn pattern from 02.6 with the separate-side movement pattern from 03.4. TEST_MODE selects one station or the combined COURSE.

Keep generated imports and Brain setup above this user code, including import math. Paste below # Begin project code, outside the generated region, replacing earlier starters.

Match motor ports, cartridges, reversal flags, inertial port and measured geometry. The example assumes equal direct-drive wheels. Avoid duplicate manual and Devices-panel definitions.

Start disabled and still, facing your chosen zero direction. Verify sensor connection and readings before trusting calibration status. The short tests begin only after setup is ready and autonomous is enabled.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER]
# =============================================================================
# ROBOT CONFIGURATION
# - FOUR-motor drivetrain:
#       Left  = Ports 3 and 4
#       Right = Ports 1 and 2
# - Example: blue cartridges, left normal, right reversed.
#   VERIFY your actual ports, cartridges and direction settings.
# DRIVER CONTROLS
# - Left joystick vertical (axis3): forward/back
# - Right joystick horizontal (axis1): turn
# - This program MOVES in autonomous and driver-control modes.
# - Inertial sensor: Port 20 (example; check your robot)
# - Autonomous: selected test station or a short combined course.
# - Hold B to cancel autonomous; keep a clear test area.
# - Example geometry assumes DIRECT drive (external ratio 1).
# - Add BELOW # Begin project code, outside generated configuration.
# - Do not also define these drive motors in the Devices panel.
# - Keep generated from vex import * and brain = Brain() above.

# =============================================================================
# FOUR-MOTOR DRIVETRAIN
# =============================================================================
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)

# =============================================================================
# MEASURED GEOMETRY: EXAMPLES, REPLACE WITH YOUR MEASUREMENTS
# =============================================================================
WHEEL_DIAMETER_MM = 101.6
WHEEL_TRAVEL_MM = math.pi * WHEEL_DIAMETER_MM
TRACK_WIDTH_MM = 300
WHEEL_BASE_MM = 250
EXTERNAL_GEAR_RATIO = 1.0  # This example is direct drive only

inertial_sensor = Inertial(Ports.PORT20)

drivetrain = SmartDrive(
    left_drive, right_drive, inertial_sensor,
    WHEEL_TRAVEL_MM, TRACK_WIDTH_MM, WHEEL_BASE_MM,
    MM, EXTERNAL_GEAR_RATIO
)

# =============================================================================
# CONTROLLER, SETTINGS AND HELPERS
# =============================================================================
controller = Controller(PRIMARY)  # Do not duplicate a generated Controller
DEADBAND = 5
DRIVE_LIMIT_PERCENT = 30          # Start low; increase only after testing
TURN_GAIN = 0.7
LOOP_DELAY_MS = 20
AUTON_SPEED_PERCENT = 20
TEST_TURN_DEGREES = 90
AUTON_TURN_PERCENT = 15
CALIBRATION_LIMIT_MS = 5000
setup_ready = False
ACTION_LIMIT_MS = 4000
POLL_MS = 20
SETTLE_MS = 100
TURN_TOLERANCE_DEGREES = 3
cancel_requested = False
TRACK_MM = TRACK_WIDTH_MM
SIDE_MAX_PERCENT = 20
ARC_LIMIT_MS = 5000
ENCODER_TOLERANCE_DEGREES = 10
REVERSE_PERCENT = 15
STRAIGHT_MM = 300
TURN_DEGREES = 90
ARC_RADIUS_MM = 400
ARC_PATH_MM = 300
REVERSE_MM = 200
PAUSE_BETWEEN_MS = 500
TEST_MODE = "STRAIGHT"       # STRAIGHT, TURN, ARC, REVERSE or COURSE
PAUSE_MS = 500

def apply_deadband(value):
    return 0 if abs(value) <= DEADBAND else value

def drive_mix(forward, turn):
    left = forward + turn
    right = forward - turn
    largest = max(100, abs(left), abs(right))
    left = left * 100 / largest
    right = right * 100 / largest
    return (left * DRIVE_LIMIT_PERCENT / 100,
            right * DRIVE_LIMIT_PERCENT / 100)

def set_side_output(side, output):
    if abs(output) < 0.01:
        side.stop(BRAKE)
    elif output > 0:
        side.spin(FORWARD, output, PERCENT)
    else:
        side.spin(REVERSE, -output, PERCENT)

def show_status(message):
    brain.screen.clear_screen()
    brain.screen.set_cursor(1, 1)
    brain.screen.print(message)

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

def autonomous_ready():
    return (setup_ready and competition.is_enabled()
            and competition.is_autonomous())

def stop_reason():
    global cancel_requested
    if controller.buttonB.pressing():
        cancel_requested = True
    if cancel_requested:
        return "CANCELLED"
    if not setup_ready:
        return "NOT_READY"
    if not (competition.is_enabled() and competition.is_autonomous()):
        return "MODE_CHANGED"
    return None

def finish_action(reason):
    drivetrain.stop(AUTON_STOP_MODE)
    show_status(reason)
    return reason

def monitored_turn(direction, angle_degrees, speed=None):
    if direction not in (LEFT, RIGHT):
        raise ValueError("Use LEFT or RIGHT")
    if (not isinstance(angle_degrees, (int, float))
            or not 0 < angle_degrees <= 180):
        raise ValueError("This starter accepts angles above 0 up to 180")
    selected = AUTON_TURN_PERCENT if speed is None else speed
    if not isinstance(selected, (int, float)) or not 0 < selected <= 100:
        raise ValueError("Speed must be above 0 and at most 100")
    reason = stop_reason()
    if reason is not None:
        return finish_action(reason)

    start_rotation = drivetrain.rotation(DEGREES)
    signed_angle = angle_degrees if direction == RIGHT else -angle_degrees
    target_rotation = start_rotation + signed_angle
    start_ms = brain.timer.time(MSEC)
    drivetrain.set_stopping(AUTON_STOP_MODE)
    drivetrain.set_turn_velocity(selected, PERCENT)
    # The software deadline should normally stop first.
    drivetrain.set_timeout(ACTION_LIMIT_MS + 500, MSEC)
    drivetrain.turn_for(direction, angle_degrees, DEGREES, wait=False)
    # Yield after starting the asynchronous command before inspecting status.
    wait(POLL_MS, MSEC)
    while True:
        reason = stop_reason()
        if reason is not None:
            return finish_action(reason)
        elapsed_ms = brain.timer.time(MSEC) - start_ms
        if elapsed_ms >= ACTION_LIMIT_MS:
            return finish_action("TIMEOUT")
        if drivetrain.is_done():
            drivetrain.stop(AUTON_STOP_MODE)
            wait(SETTLE_MS, MSEC)
            reason = stop_reason()
            if reason is not None:
                return finish_action(reason)
            if brain.timer.time(MSEC) - start_ms >= ACTION_LIMIT_MS:
                return finish_action("TIMEOUT")
            error = abs(target_rotation - drivetrain.rotation(DEGREES))
            if error <= TURN_TOLERANCE_DEGREES:
                return finish_action("TARGET_CONFIRMED")
            return finish_action("ENDED_OFF_TARGET")
        wait(POLL_MS, MSEC)

def side_plan(left_mm, right_mm, max_speed):
    for value in (left_mm, right_mm):
        if (not isinstance(value, (int, float))
                or not 0 <= value < float("inf")):
            raise ValueError("This starter accepts finite non-negative distances")
    longest = max(left_mm, right_mm)
    if longest == 0:
        raise ValueError("At least one side must move")
    if not isinstance(max_speed, (int, float)) or not 0 < max_speed <= 100:
        raise ValueError("Max speed must be above 0 and at most 100")
    left_degrees = left_mm / WHEEL_TRAVEL_MM * 360
    right_degrees = right_mm / WHEEL_TRAVEL_MM * 360
    left_speed = max_speed * left_mm / longest
    right_speed = max_speed * right_mm / longest
    return left_degrees, right_degrees, left_speed, right_speed

def arc_stop_reason():
    return stop_reason()

def drive_side_distances(left_mm, right_mm, max_speed=None, travel_direction=FORWARD):
    if travel_direction not in (FORWARD, REVERSE):
        raise ValueError("Use FORWARD or REVERSE")
    speed = SIDE_MAX_PERCENT if max_speed is None else max_speed
    left_degrees, right_degrees, left_speed, right_speed = side_plan(
        left_mm, right_mm, speed)
    reason = arc_stop_reason()
    if reason is not None:
        stop_drive()
        return reason

    motors = (left_motor_1, left_motor_2, right_motor_1, right_motor_2)
    starts = [motor.position(DEGREES) for motor in motors]
    sign = 1 if travel_direction == FORWARD else -1
    targets = (sign * left_degrees, sign * left_degrees,
               sign * right_degrees, sign * right_degrees)
    start_ms = brain.timer.time(MSEC)
    next_report_ms = start_ms
    try:
        left_drive.set_stopping(BRAKE)
        right_drive.set_stopping(BRAKE)
        left_drive.set_timeout(ARC_LIMIT_MS + 500, MSEC)
        right_drive.set_timeout(ARC_LIMIT_MS + 500, MSEC)
        # Both sides start without waiting for the first to finish.
        if left_degrees > 0:
            left_drive.spin_for(travel_direction, left_degrees, DEGREES,
                                left_speed, PERCENT, wait=False)
        else:
            left_drive.stop(BRAKE)
        if right_degrees > 0:
            right_drive.spin_for(travel_direction, right_degrees, DEGREES,
                                 right_speed, PERCENT, wait=False)
        else:
            right_drive.stop(BRAKE)
        wait(LOOP_DELAY_MS, MSEC)
        while True:
            reason = arc_stop_reason()
            if reason is not None:
                return reason
            if brain.timer.time(MSEC) - start_ms >= ARC_LIMIT_MS:
                return "TIMEOUT"
            now_ms = brain.timer.time(MSEC)
            if now_ms >= next_report_ms:
                changes = [motor.position(DEGREES) - start
                           for motor, start in zip(motors, starts)]
                left_progress = ((changes[0] + changes[1]) / 2 / (sign * left_degrees)
                                 if left_degrees > 0 else None)
                right_progress = ((changes[2] + changes[3]) / 2 / (sign * right_degrees)
                                  if right_degrees > 0 else None)
                print("PROGRESS", left_progress, right_progress)
                next_report_ms = now_ms + 200
            left_done = left_degrees == 0 or left_drive.is_done()
            right_done = right_degrees == 0 or right_drive.is_done()
            if left_done and right_done:
                stop_drive()
                wait(100, MSEC)
                reason = arc_stop_reason()
                if reason is not None:
                    return reason
                if brain.timer.time(MSEC) - start_ms >= ARC_LIMIT_MS:
                    return "TIMEOUT"
                errors = [abs((motor.position(DEGREES) - start) - target)
                          for motor, start, target in zip(motors, starts, targets)]
                if max(errors) <= ENCODER_TOLERANCE_DEGREES:
                    return "ENCODER_TARGETS_CONFIRMED"
                return "ENDED_OFF_TARGET"
            wait(LOOP_DELAY_MS, MSEC)
    finally:
        stop_drive()

def pause_between_tests():
    start_ms = brain.timer.time(MSEC)
    while brain.timer.time(MSEC) - start_ms < PAUSE_BETWEEN_MS:
        if stop_reason() is not None:
            stop_drive()
            return False
        wait(LOOP_DELAY_MS, MSEC)
    return True

def run_station(name):
    if name == "STRAIGHT":
        return drive_side_distances(STRAIGHT_MM, STRAIGHT_MM)
    if name == "TURN":
        return monitored_turn(RIGHT, TURN_DEGREES)
    if name == "ARC":
        # Forward LEFT arc: left side inside, right side outside.
        angle = ARC_PATH_MM / ARC_RADIUS_MM
        inside = angle * (ARC_RADIUS_MM - TRACK_MM / 2)
        outside = angle * (ARC_RADIUS_MM + TRACK_MM / 2)
        return drive_side_distances(inside, outside)
    if name == "REVERSE":
        return drive_side_distances(REVERSE_MM, REVERSE_MM,
                                    max_speed=REVERSE_PERCENT,
                                    travel_direction=REVERSE)
    raise ValueError("Unknown station")

# =============================================================================
# PRE-AUTON: CALLED ONCE AT PROJECT STARTUP
# =============================================================================
def pre_autonomous():
    global setup_ready
    setup_ready = False
    stop_drive()
    show_status("Keep robot still")
    inertial_sensor.calibrate()
    elapsed_ms = 0
    while inertial_sensor.is_calibrating() and elapsed_ms < CALIBRATION_LIMIT_MS:
        wait(10, MSEC)
        elapsed_ms += 10
    if inertial_sensor.is_calibrating():
        show_status("Calibration timeout")
        return
    # Place the robot facing your chosen route-zero direction first.
    drivetrain.set_heading(0, DEGREES)
    drivetrain.set_stopping(BRAKE)
    setup_ready = True
    show_status("Setup ready")

# =============================================================================
# AUTONOMOUS: A FINITE ROUTINE
# =============================================================================
def autonomous():
    global cancel_requested
    cancel_requested = False
    stations = ("STRAIGHT", "TURN", "ARC", "REVERSE") if TEST_MODE == "COURSE" else (TEST_MODE,)
    try:
        for name in stations:
            reason = stop_reason()
            if reason is not None:
                show_status(reason)
                return
            show_status(name)
            start_ms = brain.timer.time(MSEC)
            start_rotation = drivetrain.rotation(DEGREES)
            result = run_station(name)
            elapsed_ms = brain.timer.time(MSEC) - start_ms
            angle_change = drivetrain.rotation(DEGREES) - start_rotation
            print("TEST", name, result, elapsed_ms, angle_change)
            show_status(name + ": " + result)
            if result not in ("TARGET_CONFIRMED", "ENCODER_TARGETS_CONFIRMED"):
                return
            if not pause_between_tests():
                return
        show_status("Tests ended; inspect physical results")
    finally:
        stop_drive()

# =============================================================================
# DRIVER CONTROL: REPEATING UPDATES WHILE ENABLED
# =============================================================================
def driver_control():
    if not setup_ready:
        stop_drive()
        return
    show_status("Driver control")
    while competition.is_enabled() and competition.is_driver_control():
        forward = apply_deadband(controller.axis3.position())
        turn = apply_deadband(controller.axis1.position()) * TURN_GAIN
        left_output, right_output = drive_mix(forward, turn)
        set_side_output(left_drive, left_output)
        set_side_output(right_drive, right_output)
        wait(LOOP_DELAY_MS, MSEC)
    stop_drive()

# =============================================================================
# PROGRAM START: SETUP, THEN CALLBACK REGISTRATION
# =============================================================================
pre_autonomous()
competition = Competition(driver_control, autonomous)

# Keep the main task alive without running a driving loop here.
while True:
    wait(100, MSEC)

Start with TEST_MODE = "STRAIGHT", then test TURN, ARC and REVERSE independently. Switch to COURSE only after those tests have passed your physical criteria.

A failed sensor outcome, timeout or cancellation aborts the sequence. Accepted sensor outcomes allow the next step after a brief monitored pause, but the software does not inspect physical endpoint zones.

The project deliberately uses the earlier basic target controllers. It does not enable the calibration-dependent voltage profile from 03.5. Add that as a documented comparison after calibration rather than silently changing the baseline.

07 | WHAT THE PROJECT CAN CONFIRM

For straight, arc and reverse tests, each motor’s encoder change is compared with its own signed target. Reverse targets are negative changes from the starting readings.

For the turn, accumulated inertial rotation is compared with the requested change. The overall station log includes measured rotation change and elapsed time.

ENCODER_TARGETS_CONFIRMED and TARGET_CONFIRMED are sensor results. Wheels can slip, an arc can vary during travel, and an inertial sensor can be misconfigured. Confirm the ground path independently.

After a physical failure during a combined run, disable before another segment begins. The short pause is for observation, not an automatic permission checkpoint.

08 | USE A SIMPLE RESULTS LOG

For signed distance error, use measured travel magnitude minus requested travel magnitude. A +12 mm result means 12 mm too far; −12 mm means 12 mm short.

Compare heading using the shortest signed difference around zero. A heading of 359° relative to a 0° target is about −1°, not a 359° error.

Use this space while reviewing the page. Copy your notes into your notebook before closing; this box does not save automatically.

09 | MEASURE ACCURACY AND CONSISTENCY

Accuracy describes closeness to the requested result. Repeatability describes how similar repeated results are.

If three 300 mm trials give 311, 312 and 313 mm, the robot is repeatable but consistently too long. If they give 286, 300 and 314 mm, the mean is correct but the behaviour varies substantially.

Calculate mean error and the range. A consistent bias suggests calibration; large variation suggests changing traction, load, starting alignment, stopping behaviour or mechanical play.

10 | THEN RUN THE COMBINED COURSE

The combined route is straight → right turn → forward left arc → reverse. Each movement begins from the actual result of the previous movement, so errors carry forward.

Document the predicted final centre position on your layout and the approximately 47.03° final heading. Track the whole path as well as the final pose.

Do not re-zero heading between segments to make the readings look correct. Keep one reference for the trial. Re-zero only during deliberate preparation for a new run.

If isolated tests pass but the sequence fails, inspect transitions, settling, initial alignment and accumulated error before changing every helper.

11 | COMPARE IMPROVEMENTS FAIRLY

Use your baseline test log to compare a tuned stopping policy, slew limiter or calibrated profile. Keep target distances and course geometry unchanged.

For a profile comparison, retain its calibration gate, voltage clamp, cancellation, deadline and settling checks. Do not run two controllers on the same motors at once.

Record whether an improvement reduces physical error, variation, completion time or all three. A faster course is not automatically a better course if reliability decreases.

12 | TEST FAILURE BEHAVIOUR TOO

• Cancellation: hold B during autonomous and verify the route ends rather than resuming on release.
• Disable: use normal competition controls and verify movement stops.
• Timeout: temporarily shorten a deadline during a controlled low-speed trial; confirm no next station starts.
• Preparation failure: verify that the routine refuses to start when setup is not ready.
• Invalid configuration: inspect settings and sensor readings before attempting floor tests.

Do not deliberately jam the drivetrain to create a fault. Use short deadlines or simulated sensor outcomes for logic tests, then restore the normal settings.

CHALLENGES | PRODUCE A MOVEMENT VALIDATION REPORT

1. Prepare: draw and label four stations with start points and clearance.
2. Predict: calculate arc side travel and expected heading change.
3. Test: complete three trials of each independent movement.
4. Analyse: calculate mean error and range and identify one systematic bias.
5. Improve: change one setting and rerun the same test.
6. Combine: run three complete courses after the stations meet your criteria.
7. Verify: demonstrate cancellation, disable and timeout behaviour.
8. Report: write a short recommendation stating the reliable settings, evidence and remaining limitations.

OFFICIAL REFERENCES

• VEX drivetrain turns and rotation readings
• VEX motor-group targets and encoder readings

This is an independent learning resource; VEX and VEXcode are trademarks of their respective owners.