Path Planning & Motion Chaining

Overview#

Basic autonomous routines drive in straight lines and turn at 90-degree angles. This works — but it is slow. Every time the robot stops at the end of a drive segment and waits to fully settle before turning, it wastes time.

Path planning and motion chaining are techniques that make autonomous routines faster and smoother by reducing or eliminating these pauses.

Why does this matter?

In a 15-second or 60-second autonomous period, saving even half a second per motion adds up. A faster robot can score more objects before time runs out — and that wins matches.

Motion Chaining#

Motion chaining means starting the next motion before the current one has fully settled. Instead of waiting for the robot to come to a complete stop, you check whether it has passed a “close enough” threshold, then immediately begin the next move.

The Problem with Full Settlement#

Here is a standard routine that waits for full settlement:

Drive 24" → [wait until fully stopped] → Turn 90° → [wait until fully stopped] → Drive 12"

Total time: 3.5 seconds

With Motion Chaining#

Drive 24" → [start turn when within 3" of target] → Drive 12"

Total time: 2.1 seconds (same path, ~40% faster)

How to Implement Motion Chaining (VEX V5 C++)#

Instead of waiting for the robot to stop, check whether it is close enough to the target position and start the next motion immediately.

// Standard: wait for full settlement
void driveForward(double targetInches, int speed) {
    // ... PD controller loop ...
    while (abs(error) > 0.5) {
        // keep running PD
        wait(20, msec);
    }
    LeftMotor.stop(brake);
    RightMotor.stop(brake);
}

// Chained: exit early when close enough
void driveForwardChained(double targetInches, int speed, double exitThreshold = 3.0) {
    // ... PD controller loop ...
    while (abs(error) > exitThreshold) {  // exits earlier!
        // keep running PD
        wait(20, msec);
    }
    // Do NOT brake — let momentum carry the robot into the turn
}

The key change is replacing the tight 0.5 inch exit threshold with a larger 3.0 inch threshold. The robot starts the next motion while it still has some momentum left.

When NOT to Chain#

Do not chain motions when:

• You need to pick up an object at an exact position (the robot must be fully stopped)
• You are scoring into a goal that requires precision
• The next motion involves a sensor-triggered action that requires a stable reading

Curved Path Approximation#

Real competition fields often require curved paths — sweeping around a field element, arcing toward a goal. You can approximate curves using a series of very short straight segments with small turns between them.

The Idea#

Instead of one big 90-degree arc, drive twenty 4.5-degree turns with short drives between them. At full speed, this looks like a smooth curve.

// VEX V5 C++ — approximate an arc
// radius: arc radius in inches
// degrees: total arc angle to travel
void arcDrive(double radiusInches, double arcDegrees) {
    int steps = 20;  // number of segments to approximate the arc
    double anglePerStep = arcDegrees / steps;
    double arcLength = (3.14159 * radiusInches * arcDegrees) / 180.0;
    double distPerStep = arcLength / steps;

    for (int i = 0; i < steps; i++) {
        driveForwardChained(distPerStep, 80, 0.5);  // very small exit threshold
        turnToHeading(currentHeading + anglePerStep);
    }
}

Measuring Arc Accuracy#

After implementing arcDrive, test it on the actual field and measure where the robot ends up. Compare that to where it should end up geometrically. Record your error in the notebook and adjust the number of steps or speed to improve accuracy.

Error Recovery#

Even the most carefully tuned autonomous routine sometimes fails. A wheel slips, a game object is not where you expected it, or the inertial sensor drifts slightly. Without error recovery, the robot freezes or continues executing commands that no longer make sense.

Error recovery detects when something has gone wrong and takes a corrective action rather than giving up.

Timeout-Based Recovery#

Wrap each autonomous action in a time limit. If the robot does not complete the action within that limit, something has gone wrong.

// VEX V5 C++ — drive with timeout
void driveForwardSafe(double targetInches, int speed, double timeoutSeconds = 3.0) {
    Brain.Timer.clear();

    while (abs(error) > 1.0) {
        // Check timeout
        if (Brain.Timer.value() > timeoutSeconds) {
            Brain.Screen.print("TIMEOUT — recovering");
            LeftMotor.stop(brake);
            RightMotor.stop(brake);
            return;  // exit the function and continue with the next step
        }

        // Normal PD controller logic...
        wait(20, msec);
    }

    LeftMotor.stop(brake);
    RightMotor.stop(brake);
}

Setting timeoutSeconds to 1.5× the expected time is a good starting point.

Recovery Moves#

When a timeout triggers, you can also attempt a recovery move — a simple backup or reset that gets the robot into a known state.

void recoverFromStall() {
    // Back up a few inches and try again
    driveForwardSafe(-6, 50, 1.5);
    turnToHeading(inertialSensor.heading());  // straighten out
}

Autonomous Routine Menu#

At competition, you may want to pick between routines without reprogramming. Use the Brain touchscreen to build a simple selection menu in pre_auton().

// VEX V5 C++ — three-routine selector
int selectedRoutine = 1;

void pre_auton() {
    Brain.Screen.clearScreen();

    while (!Controller1.ButtonA.pressing()) {
        Brain.Screen.clearScreen();
        Brain.Screen.setCursor(1, 1);
        Brain.Screen.print("Routine: %d", selectedRoutine);
        Brain.Screen.setCursor(2, 1);
        Brain.Screen.print("L/R to change, A to confirm");

        if (Controller1.ButtonLeft.pressing()) {
            selectedRoutine = max(1, selectedRoutine - 1);
            wait(300, msec);
        } else if (Controller1.ButtonRight.pressing()) {
            selectedRoutine = min(3, selectedRoutine + 1);
            wait(300, msec);
        }
        wait(50, msec);
    }

    // Calibrate inertial sensor
    InertialSensor.calibrate();
    wait(2000, msec);
}

void autonomous() {
    if      (selectedRoutine == 1) routinePrimary();
    else if (selectedRoutine == 2) routineBackup();
    else if (selectedRoutine == 3) routineSkills();
}

Always include a “do nothing” option (routine 0 or a special case) in case you need to stay out of your alliance partner’s way.

Testing Your Path#

Follow these steps when testing a new path:

1. Mark the starting position with tape — Place the robot in the exact same spot every run.
2. Run 3 times at reduced speed (50%) — Verify the robot follows the intended path before testing at full speed.
3. Run 5 times at full speed — Record where the robot ends up each time.
4. Measure end position error — How far from the intended final position is the robot?
5. If error is consistent (always 3 inches to the left), adjust constants. If error is random, look for mechanical issues (loose wheel, slipping gear, worn motor cartridge).

Related Topics#

• Navigation: PID Controllers
• Navigation: Odometry
• Navigation: Autonomous Strategy
• Software: Competition Template