Skip to content
CrocoDocs
Starter GuideProgrammingEngineeringDesignPortfolio
Documentation

Introduction

IntroductionBIOBUZZ game overview

Getting started

Getting startedBlocksOnBot JavaAndroid StudioYour first OpModeJava basicsMotors and servosNonblocking codeDebugging robot code

Control systems

Control systemsJoystick mappingField-centric mecanum drivePID controlFeedforwardFlywheel velocity controlSystem identificationMotion profilingKalman filterLow-pass filter

Autonomous

AutonomousTimed and encoder movementOdometryLocalization troubleshootingMotion planningReliable autonomous sequencesPure PursuitSensor fusionAuto to TeleOp handoff

Season topics

Season topicsProjectile mathematicsTurret aimingShooting on the move

Code practices

Code practicesNaming conventionsCode organizationLoop timing and bulk readsFormattingComments and documentationTeam collaboration

Libraries

LibrariesNextFTCFTCLibSolversLibPedro PathingRoad RunnerFTC DashboardMercurial v2Mercurial v1PanelsSloth

Community software

Community softwareBlaze FTCOpenLoopHive Vision

Sensors and vision

Vision overviewVision basicsLimelight 3A
Starter Guide
Engineering
Design
Portfolio
Appearance

Motion planning

Turn an autonomous goal into safe, testable robot movements

Motion planning is deciding which places the must visit, and in what order. It is separate from localization, which estimates where the robot is, and control, which moves it toward its next target.

This lesson starts with a simple route: drive forward to a scoring location, operate a claw, then back away to a parking location. The example uses -based moves and a small helper on a tank ; a field-ready curved path also needs a calibrated localizer and a path-following library.

Plan an autonomous route as a sequence of start, score, and park goals.

Route plan

Describe the route in plain language first:

  1. Start at the marked starting pose.
  2. Move forward to the scoring area.
  3. Open the claw and give the game piece time to release.
  4. Back away to the parking area.

This list is an action sequence. Each movement should have a distance or target pose, a speed limit, and a condition that says when it is done. Keep the first route short enough to test on and in a clear practice area.

Encoder scale

The code needs COUNTS_PER_INCH for the actual drivetrain. Measure it by driving a known distance slowly, reading the encoder change, and dividing ticks by inches. Do not copy another robot's number: gearing, wheel size, and load change the result.

The example below expects both drive encoders to increase when the robot moves forward. Fix direction or encoder sign during a low-power check if that is not true.

Movement helper

This first implementation uses RUN_TO_POSITION. The motors handle the encoder target; the checks whether both sides finished before starting the next action. RUN_TO_POSITION is useful for simple straight moves, but it does not know the robot's field pose and wheel slip can still change where the robot ends up.

Add this helper inside your LinearOpMode class. The is a safeguard; adjust it only after measuring a safe expected move time.

private static final double COUNTS_PER_INCH = 50.0; // Measure on your robot
private DcMotor leftDrive;
private DcMotor rightDrive;

private boolean driveInches(double inches, double power) {
    if (!opModeIsActive()) return false;
    int leftTarget = leftDrive.getCurrentPosition()
        + (int) Math.round(inches * COUNTS_PER_INCH);
    int rightTarget = rightDrive.getCurrentPosition()
        + (int) Math.round(inches * COUNTS_PER_INCH);

    leftDrive.setTargetPosition(leftTarget);
    rightDrive.setTargetPosition(rightTarget);
    leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    leftDrive.setPower(Math.abs(power));
    rightDrive.setPower(Math.abs(power));

    ElapsedTime timer = new ElapsedTime();
    while (opModeIsActive()
            && (leftDrive.isBusy() || rightDrive.isBusy())
            && timer.seconds() < 4.0) {
        telemetry.addData("Left ticks", leftDrive.getCurrentPosition());
        telemetry.addData("Right ticks", rightDrive.getCurrentPosition());
        telemetry.update();
        idle();
    }

    boolean reachedTarget = !leftDrive.isBusy() && !rightDrive.isBusy();
    leftDrive.setPower(0.0);
    rightDrive.setPower(0.0);
    leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    return opModeIsActive() && reachedTarget;
}
private val COUNTS_PER_INCH = 50.0 // Measure on your robot
private lateinit var leftDrive: DcMotor
private lateinit var rightDrive: DcMotor

private fun driveInches(inches: Double, power: Double): Boolean {
    if (!opModeIsActive()) return false
    val leftTarget = leftDrive.currentPosition + (inches * COUNTS_PER_INCH).roundToInt()
    val rightTarget = rightDrive.currentPosition + (inches * COUNTS_PER_INCH).roundToInt()

    leftDrive.targetPosition = leftTarget
    rightDrive.targetPosition = rightTarget
    leftDrive.mode = DcMotor.RunMode.RUN_TO_POSITION
    rightDrive.mode = DcMotor.RunMode.RUN_TO_POSITION
    leftDrive.power = abs(power)
    rightDrive.power = abs(power)

    val timer = ElapsedTime()
    while (opModeIsActive() && (leftDrive.isBusy || rightDrive.isBusy) && timer.seconds() < 4.0) {
        telemetry.addData("Left ticks", leftDrive.currentPosition)
        telemetry.addData("Right ticks", rightDrive.currentPosition)
        telemetry.update()
        idle()
    }

    val reachedTarget = !leftDrive.isBusy && !rightDrive.isBusy
    leftDrive.power = 0.0
    rightDrive.power = 0.0
    leftDrive.mode = DcMotor.RunMode.RUN_USING_ENCODER
    rightDrive.mode = DcMotor.RunMode.RUN_USING_ENCODER
    return opModeIsActive() && reachedTarget
}

Read the helper in four parts: calculate targets from the current counts, ask the motors to run to those targets, keep the OpMode responsive while they move, and stop/restore the motors before returning. Passing a negative distance makes both targets lower than their current counts, so the robot backs up. Your motor directions and encoder signs must be verified for that to mean physical reverse.

Complete OpMode

This example assumes configured hardware names leftDrive, rightDrive, and claw. Change them to match Robot Configuration. Set each motor direction for your drivetrain; the direction shown is only an example.

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;

@Autonomous(name = "Plan: Score and Park")
public class ScoreAndParkAuto extends LinearOpMode {
    private static final double COUNTS_PER_INCH = 50.0; // Measure on your robot
    private DcMotor leftDrive;
    private DcMotor rightDrive;
    private Servo claw;

    @Override
    public void runOpMode() {
        leftDrive = hardwareMap.get(DcMotor.class, "leftDrive");
        rightDrive = hardwareMap.get(DcMotor.class, "rightDrive");
        claw = hardwareMap.get(Servo.class, "claw");

        leftDrive.setDirection(DcMotorSimple.Direction.REVERSE); // Verify on blocks
        rightDrive.setDirection(DcMotorSimple.Direction.FORWARD); // Verify on blocks
        leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

        telemetry.addLine("Check the robot is at its marked start pose.");
        telemetry.update();
        waitForStart();
        if (!opModeIsActive()) return;

        try {
        // Route action 1: travel to the scoring area.
        if (!driveInches(24.0, 0.30)) return;

        // Route action 2: release the game piece.
        claw.setPosition(0.75); // Measure safe claw positions for your linkage.
        sleep(300);
        if (!opModeIsActive()) return;

        // Route action 3: back away toward the parking area.
        driveInches(-12.0, 0.25);
        } finally {
            leftDrive.setPower(0.0);
            rightDrive.setPower(0.0);
        }
    }

    private boolean driveInches(double inches, double power) {
    if (!opModeIsActive()) return false;
        int leftTarget = leftDrive.getCurrentPosition()
            + (int) Math.round(inches * COUNTS_PER_INCH);
        int rightTarget = rightDrive.getCurrentPosition()
            + (int) Math.round(inches * COUNTS_PER_INCH);

        leftDrive.setTargetPosition(leftTarget);
        rightDrive.setTargetPosition(rightTarget);
        leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        leftDrive.setPower(Math.abs(power));
        rightDrive.setPower(Math.abs(power));

        ElapsedTime timer = new ElapsedTime();
        while (opModeIsActive()
                && (leftDrive.isBusy() || rightDrive.isBusy())
                && timer.seconds() < 4.0) {
            telemetry.addData("Target inches", "%.1f", inches);
            telemetry.addData("Left ticks", leftDrive.getCurrentPosition());
            telemetry.addData("Right ticks", rightDrive.getCurrentPosition());
            telemetry.update();
            idle();
        }

        boolean reachedTarget = !leftDrive.isBusy() && !rightDrive.isBusy();
        leftDrive.setPower(0.0);
        rightDrive.setPower(0.0);
        leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        return opModeIsActive() && reachedTarget;
    }
}
import com.qualcomm.robotcore.eventloop.opmode.Autonomous
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
import com.qualcomm.robotcore.hardware.DcMotor
import com.qualcomm.robotcore.hardware.DcMotorSimple
import com.qualcomm.robotcore.hardware.Servo
import com.qualcomm.robotcore.util.ElapsedTime
import kotlin.math.abs
import kotlin.math.roundToInt

@Autonomous(name = "Plan: Score and Park")
class ScoreAndParkAuto : LinearOpMode() {
    companion object { private const val COUNTS_PER_INCH = 50.0 }

    private lateinit var leftDrive: DcMotor
    private lateinit var rightDrive: DcMotor
    private lateinit var claw: Servo

    override fun runOpMode() {
        leftDrive = hardwareMap.get(DcMotor::class.java, "leftDrive")
        rightDrive = hardwareMap.get(DcMotor::class.java, "rightDrive")
        claw = hardwareMap.get(Servo::class.java, "claw")

        leftDrive.direction = DcMotorSimple.Direction.REVERSE // Verify on blocks
        rightDrive.direction = DcMotorSimple.Direction.FORWARD // Verify on blocks
        leftDrive.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
        rightDrive.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
        leftDrive.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER
        rightDrive.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER
        leftDrive.mode = DcMotor.RunMode.RUN_USING_ENCODER
        rightDrive.mode = DcMotor.RunMode.RUN_USING_ENCODER

        telemetry.addLine("Check the robot is at its marked start pose.")
        telemetry.update()
        waitForStart()
        if (!opModeIsActive()) return

        try {
        if (!driveInches(24.0, 0.30)) return
        claw.position = 0.75 // Measure safe claw positions for your linkage.
        sleep(300)
        if (!opModeIsActive()) return
        driveInches(-12.0, 0.25)
        } finally {
            leftDrive.power = 0.0
            rightDrive.power = 0.0
        }
    }

    private fun driveInches(inches: Double, power: Double): Boolean {
    if (!opModeIsActive()) return false
        val leftTarget = leftDrive.currentPosition + (inches * COUNTS_PER_INCH).roundToInt()
        val rightTarget = rightDrive.currentPosition + (inches * COUNTS_PER_INCH).roundToInt()
        leftDrive.targetPosition = leftTarget
        rightDrive.targetPosition = rightTarget
        leftDrive.mode = DcMotor.RunMode.RUN_TO_POSITION
        rightDrive.mode = DcMotor.RunMode.RUN_TO_POSITION
        leftDrive.power = abs(power)
        rightDrive.power = abs(power)

        val timer = ElapsedTime()
        while (opModeIsActive() && (leftDrive.isBusy || rightDrive.isBusy) && timer.seconds() < 4.0) {
            telemetry.addData("Target inches", "%.1f", inches)
            telemetry.addData("Left ticks", leftDrive.currentPosition)
            telemetry.addData("Right ticks", rightDrive.currentPosition)
            telemetry.update()
            idle()
        }

        val reachedTarget = !leftDrive.isBusy && !rightDrive.isBusy
        leftDrive.power = 0.0
        rightDrive.power = 0.0
        leftDrive.mode = DcMotor.RunMode.RUN_USING_ENCODER
        rightDrive.mode = DcMotor.RunMode.RUN_USING_ENCODER
        return opModeIsActive() && reachedTarget
    }
}

If a move times out, driveInches returns false and the route ends. The entry guard prevents a new move after STOP, including STOP during sleep(). The outer finally removes drive power on every exit. Check the SDK's DcMotor sample for API details in your installed SDK version.

Turns and curves

The example only drives straight. To add a turn, measure the robot's heading with an and close the loop on a target angle, or calibrate an encoder turn carefully. Two straight encoder moves do not establish an accurate field path: wheel slip makes the position estimate drift.

For smooth curves or routes defined in field coordinates, first calibrate localization, then use a maintained library such as Pedro Pathing or Road Runner. Those libraries take poses and paths and update a follower each loop. Their current setup steps are version-specific; use the matching official Quickstart rather than copying old examples.

State machines

The example above waits for each straight move to finish. That is easy to read, but it also blocks other work while driving. A larger routine can instead use a state machine: each loop performs a small amount of work, then changes to the next state when its condition is met. This lets the robot drive while an or arm runs at the same time.

To see the difference, here is the same drive, release, and park routine written as a non-blocking encoder state machine. beginDriveInches() starts a move and returns immediately. Each pass through the loop checks whether that move finished, so the OpMode can also read or update an arm while the robot is driving.

Add these fields and helpers to the LinearOpMode class shown above. They use the same motors and COUNTS_PER_INCH value:

private enum AutoStep { DRIVE_TO_SCORE, RELEASE, DRIVE_TO_PARK, DONE }
private final ElapsedTime moveTimer = new ElapsedTime();
private boolean moveTimedOut;

private void beginDriveInches(double inches, double power) {
    if (!opModeIsActive()) return;
    int leftTarget = leftDrive.getCurrentPosition()
        + (int) Math.round(inches * COUNTS_PER_INCH);
    int rightTarget = rightDrive.getCurrentPosition()
        + (int) Math.round(inches * COUNTS_PER_INCH);

    leftDrive.setTargetPosition(leftTarget);
    rightDrive.setTargetPosition(rightTarget);
    leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    leftDrive.setPower(Math.abs(power));
    rightDrive.setPower(Math.abs(power));
    moveTimedOut = false;
    moveTimer.reset();
}

private boolean driveMoveFinished() {
    boolean atTarget = !leftDrive.isBusy() && !rightDrive.isBusy();
    if (!atTarget && moveTimer.seconds() < 4.0) return false;

    moveTimedOut = !atTarget;
    stopDrive();
    return true;
}

private void stopDrive() {
    leftDrive.setPower(0.0);
    rightDrive.setPower(0.0);
    leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
private enum class AutoStep { DRIVE_TO_SCORE, RELEASE, DRIVE_TO_PARK, DONE }
private val moveTimer = ElapsedTime()
private var moveTimedOut = false

private fun beginDriveInches(inches: Double, power: Double) {
    if (!opModeIsActive()) return
    val leftTarget = leftDrive.currentPosition + (inches * COUNTS_PER_INCH).roundToInt()
    val rightTarget = rightDrive.currentPosition + (inches * COUNTS_PER_INCH).roundToInt()
    leftDrive.targetPosition = leftTarget
    rightDrive.targetPosition = rightTarget
    leftDrive.mode = DcMotor.RunMode.RUN_TO_POSITION
    rightDrive.mode = DcMotor.RunMode.RUN_TO_POSITION
    leftDrive.power = abs(power)
    rightDrive.power = abs(power)
    moveTimedOut = false
    moveTimer.reset()
}

private fun driveMoveFinished(): Boolean {
    val atTarget = !leftDrive.isBusy && !rightDrive.isBusy
    if (!atTarget && moveTimer.seconds() < 4.0) return false
    moveTimedOut = !atTarget
    stopDrive()
    return true
}

private fun stopDrive() {
    leftDrive.power = 0.0
    rightDrive.power = 0.0
    leftDrive.mode = DcMotor.RunMode.RUN_USING_ENCODER
    rightDrive.mode = DcMotor.RunMode.RUN_USING_ENCODER
}

The enum belongs at class scope, alongside the fields, not inside runOpMode(). Then replace the blocking route after waitForStart() with this loop (keep the hardware setup above it):

if (!opModeIsActive()) return;
AutoStep step = AutoStep.DRIVE_TO_SCORE;
ElapsedTime stateTimer = new ElapsedTime();

try {
    beginDriveInches(24.0, 0.30); // Start the first movement once.

    while (opModeIsActive() && step != AutoStep.DONE) {
        switch (step) {
            case DRIVE_TO_SCORE:
                if (driveMoveFinished()) {
                    if (moveTimedOut) {
                        telemetry.addLine("Score movement timed out; ending route.");
                        step = AutoStep.DONE;
                    } else {
                        claw.setPosition(0.75);
                        stateTimer.reset();
                        step = AutoStep.RELEASE;
                    }
                }
                break;

            case RELEASE:
                if (stateTimer.seconds() >= 0.30) {
                    beginDriveInches(-12.0, 0.25);
                    step = AutoStep.DRIVE_TO_PARK;
                }
                break;

            case DRIVE_TO_PARK:
                if (driveMoveFinished()) {
                    if (moveTimedOut) telemetry.addLine("Park movement timed out.");
                    step = AutoStep.DONE;
                }
                break;

            case DONE:
                break;
        }

        telemetry.addData("Step", step);
        telemetry.addData("Move time", "%.1f s", moveTimer.seconds());
        telemetry.update();
        idle();
    }
} finally {
    stopDrive();
}
if (!opModeIsActive()) return
var step = AutoStep.DRIVE_TO_SCORE
val stateTimer = ElapsedTime()

try {
    beginDriveInches(24.0, 0.30) // Start the first movement once.
    while (opModeIsActive() && step != AutoStep.DONE) {
        when (step) {
            AutoStep.DRIVE_TO_SCORE -> if (driveMoveFinished()) {
                if (moveTimedOut) {
                    telemetry.addLine("Score movement timed out; ending route.")
                    step = AutoStep.DONE
                } else {
                    claw.position = 0.75
                    stateTimer.reset()
                    step = AutoStep.RELEASE
                }
            }
            AutoStep.RELEASE -> if (stateTimer.seconds() >= 0.30) {
                beginDriveInches(-12.0, 0.25)
                step = AutoStep.DRIVE_TO_PARK
            }
            AutoStep.DRIVE_TO_PARK -> if (driveMoveFinished()) {
                if (moveTimedOut) telemetry.addLine("Park movement timed out.")
                step = AutoStep.DONE
            }
            AutoStep.DONE -> Unit
        }

        telemetry.addData("Step", step)
        telemetry.addData("Move time", "%.1f s", moveTimer.seconds())
        telemetry.update()
        idle()
    }
} finally {
    stopDrive()
}

Follow the flow: start a move once, check it on each loop, start the claw timer when the move ends, then start the next move when that timer expires. The four-second timeout is only a cautious example and must be chosen for your robot. Test each distance and timeout at low power. Because the loop keeps running during a move, you can add other small updates there, such as reading an arm encoder; do not start the same drive command again on every loop. For curved field paths, the Pedro Pathing guide shows a maintained library follower that also updates continuously.

Route testing

  1. Run the route with the robot raised and verify each wheel turns in the expected direction.
  2. On the floor, test only the first short movement at low power.
  3. Measure the actual distance and calibrate COUNTS_PER_INCH.
  4. Add one route action at a time and verify the robot really reached the intended location.
  5. Repeat the whole route from the marked start with a charged battery and the expected game-piece load.

An encoder reaching its target does not prove the robot arrived at the right field coordinate. Check the physical endpoint, localization drift, and whether the route leaves enough room for the robot's full size.

Further reading

  • Odometry — estimate field position from movement.
  • Pure Pursuit — understand one path-following approach.
  • Pedro Pathing — implement paths with a current FTC library.
  • FTC Dashboard — inspect telemetry while tuning.
  • Reliable autonomous sequences — require settled arrival and distinguish success from a timeout before scoring.
  • Auto to TeleOp handoff — carry a valid final estimate into driver control without resetting the field heading.

Sources

  • Road Runner

Localization troubleshooting

Separate scale, offset, heading, vibration, and stale-data failures before retuning a path follower

Reliable autonomous sequences

Keep paths and mechanisms updating, distinguish success from timeout, and recover when an action cannot finish

On this page

Route planEncoder scaleMovement helperComplete OpModeTurns and curvesState machinesRoute testingFurther readingSources