Motion profiling

Imagine telling a to move forward 48 inches. If the code immediately asks for full power, the wheels may slip at the start and the robot may overshoot at the end. A motion profile gives the robot a speed target for each moment: speed up, cruise if there is room, then slow down before reaching the target.

This page first works through a trapezoidal profile, then samples it in and uses the sampled target in a simple feedback loop. The distance and gains are examples: measure your encoder scale and tune on your robot before driving on a field.

A trapezoidal profile ramps speed up, cruises, then ramps it down.

Profile and controller

The profile answers: “Where should the robot be, and how fast should it be moving, right now?” A controller compares that target with encoder measurements and adjusts motor power.

The profile does not know whether the wheels slipped or the robot hit an obstacle. Feedback from encoders is what lets the controller correct some of those differences.

Units and limits

Choose one distance unit and use it everywhere. This example uses inches and seconds:

Target distance: 48 in
Maximum speed: 24 in/s
Maximum acceleration: 24 in/s²

These are easy numbers for the example, not recommended values for your robot. Start with a low speed and acceleration in a clear area, then measure what the robot can do without wheel slip.

Trapezoidal profile

At constant acceleration, speed and distance are related by:

t_\text{accel} = \frac{v_\text{max}}{a_\text{max}}, \qquad d_\text{accel} = \frac{v_\text{max}^2}{2a_\text{max}}

For the example, reaching 24 in/s at 24 in/s² takes 1 second and 12 inches. The robot also needs 12 inches to slow down. That leaves 48 - 12 - 12 = 24 inches to cruise.

The whole move is therefore:

Part	Time	Distance	What the robot does
Accelerate	1 s	12 in	Speed increases from 0 to 24 in/s
Cruise	1 s	24 in	Speed stays at 24 in/s
Decelerate	1 s	12 in	Speed decreases to 0 at the target

The velocity graph has a flat top, making a trapezoid.

Triangular profile

A short move may not leave enough distance to reach maximum speed. For example, an 8-inch move with the same acceleration limit cannot spend 12 inches speeding up and another 12 slowing down.

Instead, the robot speeds up to a lower peak and immediately slows down. The graph looks like a triangle. A correct profile generator must handle this case; simply using the maximum speed for every move can produce impossible targets and overshoot.

Profile generator

The helper below creates a profile for a move that starts and ends at rest. It automatically makes a triangular profile for short moves and a trapezoid when there is room to cruise. Position is measured from the move's starting point, in inches; velocity is inches per second.

class ProfileState {
    final double position;
    final double velocity;

    ProfileState(double position, double velocity) {
        this.position = position;
        this.velocity = velocity;
    }
}

class TrapezoidProfile {
    private final double distance;
    private final double acceleration;
    private final double peakVelocity;
    private final double accelerationTime;
    private final double cruiseTime;
    private final double totalTime;

    TrapezoidProfile(double distance, double maxVelocity, double maxAcceleration) {
        if (!Double.isFinite(distance) || distance < 0.0
                || !Double.isFinite(maxVelocity) || maxVelocity <= 0.0
                || !Double.isFinite(maxAcceleration) || maxAcceleration <= 0.0) {
            throw new IllegalArgumentException("Finite distance >= 0 and positive limits required");
        }
        this.distance = distance;
        this.acceleration = maxAcceleration;

        double timeToMaxVelocity = maxVelocity / maxAcceleration;
        double distanceToMaxVelocity = 0.5 * maxAcceleration * timeToMaxVelocity * timeToMaxVelocity;
        if (2.0 * distanceToMaxVelocity >= this.distance) {
            // Short move: speed up, then immediately slow down (triangle).
            this.peakVelocity = Math.sqrt(this.distance * maxAcceleration);
            this.accelerationTime = peakVelocity / maxAcceleration;
            this.cruiseTime = 0.0;
        } else {
            // Longer move: speed up, cruise, then slow down (trapezoid).
            this.peakVelocity = maxVelocity;
            this.accelerationTime = timeToMaxVelocity;
            double cruiseDistance = this.distance - 2.0 * distanceToMaxVelocity;
            this.cruiseTime = cruiseDistance / peakVelocity;
        }
        this.totalTime = 2.0 * accelerationTime + cruiseTime;
    }

    ProfileState sample(double seconds) {
        if (!Double.isFinite(seconds)) throw new IllegalArgumentException("Finite time required");
        double t = Math.max(0.0, Math.min(seconds, totalTime));
        double accelerationDistance = 0.5 * acceleration * accelerationTime * accelerationTime;

        if (t < accelerationTime) {
            return new ProfileState(0.5 * acceleration * t * t, acceleration * t);
        }
        if (t < accelerationTime + cruiseTime) {
            double cruiseElapsed = t - accelerationTime;
            return new ProfileState(accelerationDistance + peakVelocity * cruiseElapsed, peakVelocity);
        }

        double decelerationRemaining = totalTime - t;
        return new ProfileState(
            distance - 0.5 * acceleration * decelerationRemaining * decelerationRemaining,
            acceleration * decelerationRemaining
        );
    }

    double totalTime() {
        return totalTime;
    }
}

import kotlin.math.abs
import kotlin.math.min
import kotlin.math.max
import kotlin.math.sqrt

data class ProfileState(val position: Double, val velocity: Double)

class TrapezoidProfile(distance: Double, maxVelocity: Double, maxAcceleration: Double) {
    private val distance = distance
    private val acceleration = maxAcceleration
    private val peakVelocity: Double
    private val accelerationTime: Double
    private val cruiseTime: Double
    private val totalTime: Double

    init {
        require(distance.isFinite() && distance >= 0.0)
        require(maxVelocity.isFinite() && maxVelocity > 0.0)
        require(maxAcceleration.isFinite() && maxAcceleration > 0.0)
        val timeToMaxVelocity = maxVelocity / maxAcceleration
        val distanceToMaxVelocity = 0.5 * maxAcceleration * timeToMaxVelocity * timeToMaxVelocity
        if (2.0 * distanceToMaxVelocity >= this.distance) {
            peakVelocity = sqrt(this.distance * maxAcceleration)
            accelerationTime = peakVelocity / maxAcceleration
            cruiseTime = 0.0
        } else {
            peakVelocity = maxVelocity
            accelerationTime = timeToMaxVelocity
            val cruiseDistance = this.distance - 2.0 * distanceToMaxVelocity
            cruiseTime = cruiseDistance / peakVelocity
        }
        totalTime = 2.0 * accelerationTime + cruiseTime
    }

    fun sample(seconds: Double): ProfileState {
        require(seconds.isFinite())
        val t = max(0.0, min(seconds, totalTime))
        val accelerationDistance = 0.5 * acceleration * accelerationTime * accelerationTime
        if (t < accelerationTime) {
            return ProfileState(0.5 * acceleration * t * t, acceleration * t)
        }
        if (t < accelerationTime + cruiseTime) {
            val cruiseElapsed = t - accelerationTime
            return ProfileState(accelerationDistance + peakVelocity * cruiseElapsed, peakVelocity)
        }

        val decelerationRemaining = totalTime - t
        return ProfileState(
            distance - 0.5 * acceleration * decelerationRemaining * decelerationRemaining,
            acceleration * decelerationRemaining
        )
    }

    fun totalTime() = totalTime
}

Read the constructor from top to bottom: it first checks whether there is enough distance to reach the maximum speed. sample() then returns the position and speed the should have at a given elapsed time. The profile is mathematical; it does not read an encoder or move a motor by itself.

Encoder feedback

Here is how a single motor can follow the profile. This is a learning example for an encoder-equipped linear mechanism. Set COUNTS_PER_INCH from a measured move. kP turns position error into power; kV adds power for the profile's requested speed. Both gains are robot-specific.

private static final double COUNTS_PER_INCH = 100.0; // Measure on your robot
private static final double MAX_POWER = 0.25;        // Start with a safe limit
private static final double KP = 0.04;               // Power per inch; tune slowly
private static final double KV = 0.01;               // Power per inch/second; tune slowly

// In runOpMode(), after START and a verified clear travel range:
TrapezoidProfile profile = new TrapezoidProfile(8.0, 4.0, 8.0);
if (!opModeIsActive()) return;
slide.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
int startTicks = slide.getCurrentPosition();
long startTime = System.nanoTime();

try {
    while (opModeIsActive()) {
        double elapsed = (System.nanoTime() - startTime) / 1_000_000_000.0;
        ProfileState target = profile.sample(elapsed);
        double measuredInches = (slide.getCurrentPosition() - startTicks) / COUNTS_PER_INCH;
        double error = target.position - measuredInches;

        // Forward-only example: overshoot requires a stopped recovery.
        double power = KP * error + KV * target.velocity;
        power = Math.max(0.0, Math.min(MAX_POWER, power));
        slide.setPower(power);

        telemetry.addData("Target inches", "%.2f", target.position);
        telemetry.addData("Measured inches", "%.2f", measuredInches);
        telemetry.addData("Target speed", "%.2f in/s", target.velocity);
        telemetry.addData("Motor power", "%.2f", power);
        telemetry.update();

        if (elapsed >= profile.totalTime() && Math.abs(error) < 0.25) break;
        if (elapsed >= profile.totalTime() + 2.0) {
            telemetry.addLine("Move timed out; check encoder direction and tuning.");
            telemetry.update();
            break;
        }
        idle();
    }
} finally {
    slide.setPower(0.0);
}

private const val COUNTS_PER_INCH = 100.0 // Measure on your robot
private const val MAX_POWER = 0.25        // Start with a safe limit
private const val KP = 0.04               // Power per inch; tune slowly
private const val KV = 0.01               // Power per inch/second; tune slowly

// In runOpMode(), after START and a verified clear travel range:
val profile = TrapezoidProfile(8.0, 4.0, 8.0)
if (!opModeIsActive()) return
slide.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER
val startTicks = slide.currentPosition
val startTime = System.nanoTime()

try {
    while (opModeIsActive()) {
        val elapsed = (System.nanoTime() - startTime) / 1_000_000_000.0
        val target = profile.sample(elapsed)
        val measuredInches = (slide.currentPosition - startTicks) / COUNTS_PER_INCH
        val error = target.position - measuredInches

        // Forward-only example: overshoot requires a stopped recovery.
        val power = max(0.0, min(MAX_POWER, KP * error + KV * target.velocity))
        slide.power = power

        telemetry.addData("Target inches", "%.2f", target.position)
        telemetry.addData("Measured inches", "%.2f", measuredInches)
        telemetry.addData("Target speed", "%.2f in/s", target.velocity)
        telemetry.addData("Motor power", "%.2f", power)
        telemetry.update()

        if (elapsed >= profile.totalTime() && abs(error) < 0.25) break
        if (elapsed >= profile.totalTime() + 2.0) {
            telemetry.addLine("Move timed out; check encoder direction and tuning.")
            telemetry.update()
            break
        }
        idle()
    }
} finally {
    slide.power = 0.0
}

The loop asks the profile for the target position and velocity, compares target position with the encoder reading, and combines the position correction with a speed-based term. The power clamp protects the test from an unexpectedly large request; it does not make an unsafe mechanism safe. Use a try/finally or equivalent stop cleanup in a full so the motor is set to zero when the OpMode is stopped.

startTicks makes measured displacement relative to this move, without erasing the physical encoder reference. The helper accepts finite nonnegative distance and positive finite speed/acceleration limits; it starts and ends at rest. Put the Kotlin const val declarations at file scope (or in a companion object), the profile classes outside runOpMode(), and the follower loop inside it. The follower uses abs, min, and max from kotlin.math.

This example only applies forward power. After an overshoot it cannot reverse back to target: treat that as a failed move and stop for recovery. A mechanism verified safe in both directions may use a symmetric clamp plus measured travel limits. A vertical mechanism also needs an appropriate gravity/holding policy. A power clamp alone does not guarantee physical speed or acceleration.

The example moves one mechanism in one direction. A needs a pose estimate and a way to turn field-relative target motion into wheel powers. For that job, use the Pedro Pathing guide or Road Runner guide, rather than treating this one-axis example as a complete path follower.

Profile testing

Lift the robot so its wheels can turn safely, or test a mechanism with a clear stop and no pinch hazards.
Display elapsed time, target position, measured position, target velocity, and motor power in telemetry.
Confirm that the target position starts at the measured position, increases smoothly, and ends at the destination.
Put the robot on the floor and run a short, slow move. Check for wheel slip and overshoot.
Increase acceleration or speed a little at a time. If it slips, reduce acceleration; if it stops short or drifts, inspect encoder direction and controller tuning before changing the profile.

Profiles and paths

A one-dimensional motion profile describes progress along one move. A path describes where the robot should travel on the field. A path follower may use a profile to limit speed along that path, but the ideas are not interchangeable: a profile alone does not plan turns or avoid obstacles.

For field navigation, see Motion Planning and the Pedro Pathing guide. Use those libraries' current setup instructions for your project version.

Common mistakes

Mixing units: inches, encoder ticks, and meters cannot be combined without conversion.
Setting aggressive limits from theory alone: traction and robot weight determine what works on the field.
Ignoring short moves: the profile must support a triangular shape when there is no cruise phase.
Assuming the profile corrects disturbances: encoder feedback and a controller are still needed.
Letting code continue after stop: all movement loops need to respect the OpMode stop state and leave motors safe.

Next, learn how feedback terms work in the PID arm guide, or smooth noisy readings with a low-pass filter.

A trapezoidal profile ramps speed up, cruises, then ramps it down.

Profile and controller

The profile answers: “Where should the robot be, and how fast should it be moving, right now?” A controller compares that target with encoder measurements and adjusts motor power.

The profile does not know whether the wheels slipped or the robot hit an obstacle. Feedback from encoders is what lets the controller correct some of those differences.

Units and limits

Choose one distance unit and use it everywhere. This example uses inches and seconds:

Target distance: 48 in
Maximum speed: 24 in/s
Maximum acceleration: 24 in/s²

These are easy numbers for the example, not recommended values for your robot. Start with a low speed and acceleration in a clear area, then measure what the robot can do without wheel slip.

Trapezoidal profile

At constant acceleration, speed and distance are related by:

t_\text{accel} = \frac{v_\text{max}}{a_\text{max}}, \qquad d_\text{accel} = \frac{v_\text{max}^2}{2a_\text{max}}

For the example, reaching 24 in/s at 24 in/s² takes 1 second and 12 inches. The robot also needs 12 inches to slow down. That leaves 48 - 12 - 12 = 24 inches to cruise.

The whole move is therefore:

Part	Time	Distance	What the robot does
Accelerate	1 s	12 in	Speed increases from 0 to 24 in/s
Cruise	1 s	24 in	Speed stays at 24 in/s
Decelerate	1 s	12 in	Speed decreases to 0 at the target

The velocity graph has a flat top, making a trapezoid.

Triangular profile

A short move may not leave enough distance to reach maximum speed. For example, an 8-inch move with the same acceleration limit cannot spend 12 inches speeding up and another 12 slowing down.

Profile generator

class ProfileState {
    final double position;
    final double velocity;

    ProfileState(double position, double velocity) {
        this.position = position;
        this.velocity = velocity;
    }
}

class TrapezoidProfile {
    private final double distance;
    private final double acceleration;
    private final double peakVelocity;
    private final double accelerationTime;
    private final double cruiseTime;
    private final double totalTime;

    TrapezoidProfile(double distance, double maxVelocity, double maxAcceleration) {
        if (!Double.isFinite(distance) || distance < 0.0
                || !Double.isFinite(maxVelocity) || maxVelocity <= 0.0
                || !Double.isFinite(maxAcceleration) || maxAcceleration <= 0.0) {
            throw new IllegalArgumentException("Finite distance >= 0 and positive limits required");
        }
        this.distance = distance;
        this.acceleration = maxAcceleration;

        double timeToMaxVelocity = maxVelocity / maxAcceleration;
        double distanceToMaxVelocity = 0.5 * maxAcceleration * timeToMaxVelocity * timeToMaxVelocity;
        if (2.0 * distanceToMaxVelocity >= this.distance) {
            // Short move: speed up, then immediately slow down (triangle).
            this.peakVelocity = Math.sqrt(this.distance * maxAcceleration);
            this.accelerationTime = peakVelocity / maxAcceleration;
            this.cruiseTime = 0.0;
        } else {
            // Longer move: speed up, cruise, then slow down (trapezoid).
            this.peakVelocity = maxVelocity;
            this.accelerationTime = timeToMaxVelocity;
            double cruiseDistance = this.distance - 2.0 * distanceToMaxVelocity;
            this.cruiseTime = cruiseDistance / peakVelocity;
        }
        this.totalTime = 2.0 * accelerationTime + cruiseTime;
    }

    ProfileState sample(double seconds) {
        if (!Double.isFinite(seconds)) throw new IllegalArgumentException("Finite time required");
        double t = Math.max(0.0, Math.min(seconds, totalTime));
        double accelerationDistance = 0.5 * acceleration * accelerationTime * accelerationTime;

        if (t < accelerationTime) {
            return new ProfileState(0.5 * acceleration * t * t, acceleration * t);
        }
        if (t < accelerationTime + cruiseTime) {
            double cruiseElapsed = t - accelerationTime;
            return new ProfileState(accelerationDistance + peakVelocity * cruiseElapsed, peakVelocity);
        }

        double decelerationRemaining = totalTime - t;
        return new ProfileState(
            distance - 0.5 * acceleration * decelerationRemaining * decelerationRemaining,
            acceleration * decelerationRemaining
        );
    }

    double totalTime() {
        return totalTime;
    }
}

import kotlin.math.abs
import kotlin.math.min
import kotlin.math.max
import kotlin.math.sqrt

data class ProfileState(val position: Double, val velocity: Double)

class TrapezoidProfile(distance: Double, maxVelocity: Double, maxAcceleration: Double) {
    private val distance = distance
    private val acceleration = maxAcceleration
    private val peakVelocity: Double
    private val accelerationTime: Double
    private val cruiseTime: Double
    private val totalTime: Double

    init {
        require(distance.isFinite() && distance >= 0.0)
        require(maxVelocity.isFinite() && maxVelocity > 0.0)
        require(maxAcceleration.isFinite() && maxAcceleration > 0.0)
        val timeToMaxVelocity = maxVelocity / maxAcceleration
        val distanceToMaxVelocity = 0.5 * maxAcceleration * timeToMaxVelocity * timeToMaxVelocity
        if (2.0 * distanceToMaxVelocity >= this.distance) {
            peakVelocity = sqrt(this.distance * maxAcceleration)
            accelerationTime = peakVelocity / maxAcceleration
            cruiseTime = 0.0
        } else {
            peakVelocity = maxVelocity
            accelerationTime = timeToMaxVelocity
            val cruiseDistance = this.distance - 2.0 * distanceToMaxVelocity
            cruiseTime = cruiseDistance / peakVelocity
        }
        totalTime = 2.0 * accelerationTime + cruiseTime
    }

    fun sample(seconds: Double): ProfileState {
        require(seconds.isFinite())
        val t = max(0.0, min(seconds, totalTime))
        val accelerationDistance = 0.5 * acceleration * accelerationTime * accelerationTime
        if (t < accelerationTime) {
            return ProfileState(0.5 * acceleration * t * t, acceleration * t)
        }
        if (t < accelerationTime + cruiseTime) {
            val cruiseElapsed = t - accelerationTime
            return ProfileState(accelerationDistance + peakVelocity * cruiseElapsed, peakVelocity)
        }

        val decelerationRemaining = totalTime - t
        return ProfileState(
            distance - 0.5 * acceleration * decelerationRemaining * decelerationRemaining,
            acceleration * decelerationRemaining
        )
    }

    fun totalTime() = totalTime
}

Encoder feedback

private static final double COUNTS_PER_INCH = 100.0; // Measure on your robot
private static final double MAX_POWER = 0.25;        // Start with a safe limit
private static final double KP = 0.04;               // Power per inch; tune slowly
private static final double KV = 0.01;               // Power per inch/second; tune slowly

// In runOpMode(), after START and a verified clear travel range:
TrapezoidProfile profile = new TrapezoidProfile(8.0, 4.0, 8.0);
if (!opModeIsActive()) return;
slide.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
int startTicks = slide.getCurrentPosition();
long startTime = System.nanoTime();

try {
    while (opModeIsActive()) {
        double elapsed = (System.nanoTime() - startTime) / 1_000_000_000.0;
        ProfileState target = profile.sample(elapsed);
        double measuredInches = (slide.getCurrentPosition() - startTicks) / COUNTS_PER_INCH;
        double error = target.position - measuredInches;

        // Forward-only example: overshoot requires a stopped recovery.
        double power = KP * error + KV * target.velocity;
        power = Math.max(0.0, Math.min(MAX_POWER, power));
        slide.setPower(power);

        telemetry.addData("Target inches", "%.2f", target.position);
        telemetry.addData("Measured inches", "%.2f", measuredInches);
        telemetry.addData("Target speed", "%.2f in/s", target.velocity);
        telemetry.addData("Motor power", "%.2f", power);
        telemetry.update();

        if (elapsed >= profile.totalTime() && Math.abs(error) < 0.25) break;
        if (elapsed >= profile.totalTime() + 2.0) {
            telemetry.addLine("Move timed out; check encoder direction and tuning.");
            telemetry.update();
            break;
        }
        idle();
    }
} finally {
    slide.setPower(0.0);
}

private const val COUNTS_PER_INCH = 100.0 // Measure on your robot
private const val MAX_POWER = 0.25        // Start with a safe limit
private const val KP = 0.04               // Power per inch; tune slowly
private const val KV = 0.01               // Power per inch/second; tune slowly

// In runOpMode(), after START and a verified clear travel range:
val profile = TrapezoidProfile(8.0, 4.0, 8.0)
if (!opModeIsActive()) return
slide.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER
val startTicks = slide.currentPosition
val startTime = System.nanoTime()

try {
    while (opModeIsActive()) {
        val elapsed = (System.nanoTime() - startTime) / 1_000_000_000.0
        val target = profile.sample(elapsed)
        val measuredInches = (slide.currentPosition - startTicks) / COUNTS_PER_INCH
        val error = target.position - measuredInches

        // Forward-only example: overshoot requires a stopped recovery.
        val power = max(0.0, min(MAX_POWER, KP * error + KV * target.velocity))
        slide.power = power

        telemetry.addData("Target inches", "%.2f", target.position)
        telemetry.addData("Measured inches", "%.2f", measuredInches)
        telemetry.addData("Target speed", "%.2f in/s", target.velocity)
        telemetry.addData("Motor power", "%.2f", power)
        telemetry.update()

        if (elapsed >= profile.totalTime() && abs(error) < 0.25) break
        if (elapsed >= profile.totalTime() + 2.0) {
            telemetry.addLine("Move timed out; check encoder direction and tuning.")
            telemetry.update()
            break
        }
        idle()
    }
} finally {
    slide.power = 0.0
}

Profile testing

Lift the robot so its wheels can turn safely, or test a mechanism with a clear stop and no pinch hazards.
Display elapsed time, target position, measured position, target velocity, and motor power in telemetry.
Confirm that the target position starts at the measured position, increases smoothly, and ends at the destination.
Put the robot on the floor and run a short, slow move. Check for wheel slip and overshoot.
Increase acceleration or speed a little at a time. If it slips, reduce acceleration; if it stops short or drifts, inspect encoder direction and controller tuning before changing the profile.

Profiles and paths

For field navigation, see Motion Planning and the Pedro Pathing guide. Use those libraries' current setup instructions for your project version.

Common mistakes

Mixing units: inches, encoder ticks, and meters cannot be combined without conversion.
Setting aggressive limits from theory alone: traction and robot weight determine what works on the field.
Ignoring short moves: the profile must support a triangular shape when there is no cruise phase.
Assuming the profile corrects disturbances: encoder feedback and a controller are still needed.
Letting code continue after stop: all movement loops need to respect the OpMode stop state and leave motors safe.

Next, learn how feedback terms work in the PID arm guide, or smooth noisy readings with a low-pass filter.

Profile and controller

Units and limits

Trapezoidal profile

Triangular profile

Profile generator

Encoder feedback

Profile testing

Profiles and paths

Common mistakes

On this page

Motion profiling

Profile and controller

Units and limits

Trapezoidal profile

Triangular profile

Profile generator

Encoder feedback

Profile testing

Profiles and paths

Common mistakes

On this page