Odyssey Quickstart: Follow a Bézier path in one OpMode

This guide takes you from zero to a working autonomous OpMode that drives your FTC mecanum robot along a single cubic Bézier curve. By the end you will have a LinearOpMode that constructs a path, builds a velocity profile, spins up a Follower, and issues corrected DriveSignal values to MecanumDrive every loop iteration. Subsequent guides cover multi-segment paths, tuning, and the path editor GUI.

Add the odyssey-core dependency

Open TeamCode/build.gradle and add the project dependency inside the dependencies block. If you followed the installation guide, odyssey-core is already a sibling module registered in settings.gradle.

// TeamCode/build.gradle — dependencies block
dependencies {
    implementation project(':FtcRobotController')
    implementation project(':odyssey-core')   // ← add this line
}

Sync Gradle (Android Studio toolbar → Elephant/Sync Now) before proceeding.

Construct a BezierCurve

A cubic Bézier curve is defined by four Vector2d control points: the start point p0, two interior control handles p1 and p2, and the end point p3. Coordinates are in millimetres and match the values your Localizer reports.

Choose a HeadingInterpolator to describe how the robot’s facing angle changes along the curve. LinearInterpolator smoothly sweeps from one absolute heading to another and is the easiest starting point.

import org.firstinspires.ftc.teamcode.odyssey.geometry.Vector2d;
import org.firstinspires.ftc.teamcode.odyssey.path.BezierCurve;
import org.firstinspires.ftc.teamcode.odyssey.path.heading.LinearInterpolator;

// Control points (x, y) in millimetres — field frame
Vector2d p0 = new Vector2d(0,    0);     // start
Vector2d p1 = new Vector2d(300,  0);     // first handle
Vector2d p2 = new Vector2d(600,  400);   // second handle
Vector2d p3 = new Vector2d(900,  400);   // end

// Sweep heading from 0 radians (facing +X) to π/2 radians (facing +Y)
LinearInterpolator headingInterp = new LinearInterpolator(0, Math.PI / 2);

BezierCurve curve = new BezierCurve(p0, p1, p2, p3, headingInterp);

Wrap the curve in a Path

Path accepts one or more BezierCurve arguments (varargs). It caches each segment’s arc length at construction time so that all subsequent distance queries are fast.

import org.firstinspires.ftc.teamcode.odyssey.path.Path;

Path path = new Path(curve);
// For a multi-segment path:
// Path path = new Path(curveA, curveB, curveC);

Create a VelocityProfile

VelocityProfile pre-computes the full speed schedule at construction time. Pass the Path and your robot’s physical motion limits. step is the distance interval (mm) between sample points — smaller values give higher fidelity at the cost of more memory and a longer constructor.

import org.firstinspires.ftc.teamcode.odyssey.path.VelocityProfile;

double maxVelocity        = 600.0;  // mm/s — robot's straight-line top speed
double maxAcceleration    = 800.0;  // mm/s² — forward acceleration limit
double maxBrake           = 800.0;  // mm/s² — braking deceleration limit
double maxCentripetalAccel = 400.0; // mm/s² — lateral traction budget
double step               = 10.0;   // mm — profile resolution

VelocityProfile velocityProfile = new VelocityProfile(
    path,
    maxVelocity,
    maxAcceleration,
    maxBrake,
    maxCentripetalAccel,
    step
);

Create two PIDControllers

Follower needs one PIDController for translational cross-track error (how far the robot is from the path) and one for heading error. Both controllers use a setpoint of 0.0 — errors are computed internally and passed as currentValue on each update() call.

import org.firstinspires.ftc.teamcode.odyssey.control.PIDController;

// Translational PID — corrects lateral deviation from the path (mm)
PIDController pidTranslational = new PIDController(0.0, 0.004, 0.0, 0.0001);

// Heading PID — corrects angular deviation from the interpolated heading (rad)
PIDController pidHeading = new PIDController(0.0, 2.5, 0.0, 0.05);

Start tuning with kP only (kI = 0, kD = 0). Raise kP on the translational controller until the robot hugs the path without oscillating, then raise kD to damp overshoot. Do the same for the heading controller. Only add kI if you see a persistent steady-state offset. Full tuning instructions are in the tuning guide.

Instantiate a Follower

Follower ties the path, localizer, velocity profile, and PID controllers together. minDriveSpeed is a floor speed (mm/s) applied while distanceRemaining > floorCutoff so the robot does not stall on slow profile segments. floorCutoff (mm) disables the floor speed near the end so the robot decelerates smoothly to a stop.

import org.firstinspires.ftc.teamcode.odyssey.follower.Follower;
import org.firstinspires.ftc.teamcode.odyssey.localization.Localizer;
// localizer is your Localizer implementation, e.g. PinpointLocalizer

double minDriveSpeed = 80.0;   // mm/s — minimum forward speed while on path
double floorCutoff   = 50.0;   // mm — stop flooring speed within 50 mm of end

Follower follower = new Follower(
    path,
    localizer,
    velocityProfile,
    pidTranslational,
    pidHeading,
    minDriveSpeed,
    floorCutoff
);

Wire everything into a LinearOpMode

Call localizer.update() every loop to refresh the pose, then call follower.update(elapsedTime) to get the corrected DriveSignal, and pass it directly to MecanumDrive.drive(). Finish when follower.getDistanceRemaining() drops below your arrival threshold.

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.teamcode.odyssey.control.PIDController;
import org.firstinspires.ftc.teamcode.odyssey.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.odyssey.follower.DriveSignal;
import org.firstinspires.ftc.teamcode.odyssey.follower.Follower;
import org.firstinspires.ftc.teamcode.odyssey.geometry.Vector2d;
import org.firstinspires.ftc.teamcode.odyssey.localization.PinpointLocalizer;
import org.firstinspires.ftc.teamcode.odyssey.path.BezierCurve;
import org.firstinspires.ftc.teamcode.odyssey.path.Path;
import org.firstinspires.ftc.teamcode.odyssey.path.VelocityProfile;
import org.firstinspires.ftc.teamcode.odyssey.path.heading.LinearInterpolator;

@Autonomous(name = "OdysseyQuickstart", group = "Autonomous")
public class OdysseyQuickstartOpMode extends LinearOpMode {

    @Override
    public void runOpMode() {

        // ── 1. Build path ────────────────────────────────────────────────
        BezierCurve curve = new BezierCurve(
            new Vector2d(0,   0),
            new Vector2d(300, 0),
            new Vector2d(600, 400),
            new Vector2d(900, 400),
            new LinearInterpolator(0, Math.PI / 2)
        );
        Path path = new Path(curve);

        // ── 2. Velocity profile ──────────────────────────────────────────
        VelocityProfile velocityProfile = new VelocityProfile(
            path,
            600.0,  // maxVelocity        (mm/s)
            800.0,  // maxAcceleration    (mm/s²)
            800.0,  // maxBrake           (mm/s²)
            400.0,  // maxCentripetalAccel (mm/s²)
            10.0    // step               (mm)
        );

        // ── 3. Localizer ─────────────────────────────────────────────────
        GoBildaPinpointDriver pinpoint =
            hardwareMap.get(GoBildaPinpointDriver.class, "localizer");
        PinpointLocalizer localizer = new PinpointLocalizer(pinpoint);

        // ── 4. PID controllers ───────────────────────────────────────────
        PIDController pidTranslational = new PIDController(0.0, 0.004, 0.0, 0.0001);
        PIDController pidHeading       = new PIDController(0.0, 2.5,   0.0, 0.05);

        // ── 5. Follower ──────────────────────────────────────────────────
        Follower follower = new Follower(
            path,
            localizer,
            velocityProfile,
            pidTranslational,
            pidHeading,
            80.0,   // minDriveSpeed (mm/s)
            50.0    // floorCutoff   (mm)
        );

        // ── 6. MecanumDrive ──────────────────────────────────────────────
        DcMotorEx leftFront  = hardwareMap.get(DcMotorEx.class, "leftfront");
        DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightfront");
        DcMotorEx leftBack   = hardwareMap.get(DcMotorEx.class, "leftback");
        DcMotorEx rightBack  = hardwareMap.get(DcMotorEx.class, "rightback");
        VoltageSensor voltageSensor = hardwareMap.voltageSensor.iterator().next();

        MecanumDrive mecanumDrive = new MecanumDrive(
            0.1,   // kS — static friction offset
            0.0016,// kV — velocity feedforward
            0.0,   // kA — acceleration feedforward
            7.0,   // lX — half track width (cm, used for turn geometry)
            7.0,   // lY — half wheel-base  (cm, used for turn geometry)
            leftFront, rightFront, leftBack, rightBack,
            voltageSensor
        );

        // ── 7. Run ───────────────────────────────────────────────────────
        waitForStart();

        ElapsedTime timer = new ElapsedTime();
        double arrivalThreshold = 20.0; // mm — stop when this close to end

        while (opModeIsActive()) {
            localizer.update();

            DriveSignal signal = follower.update(timer.seconds());
            mecanumDrive.drive(signal);

            double remaining = follower.getDistanceRemaining();
            telemetry.addData("Distance remaining (mm)", remaining);
            telemetry.update();

            if (remaining < arrivalThreshold) {
                break;
            }
        }

        // Stop all motors
        mecanumDrive.drive(new DriveSignal(0, 0, 0, 0, 0));
    }
}

follower.getDistanceRemaining() is valid only after the first follower.update() call. It reuses the arc-length projection already computed inside update(), so calling it every loop costs no extra work.

What to tune next

Once the robot is moving, tune in this order:

kV — zero out kS and kA, then raise kV until the robot reaches the target speed without steady-state lag.
kS — add static friction compensation so the robot starts moving immediately at low speeds.
kA — add acceleration feedforward to reduce velocity lag during ramp-up and ramp-down.
Translational kP / kD — with feedforward correct, tighten the PID for cross-track accuracy.
Heading kP / kD — tune heading last; a well-tuned translational loop makes heading easier to stabilize.

See the full tuning guide for step-by-step procedures and telemetry dashboards.

Get Started

Core Concepts

Guides

Odyssey Quickstart: Follow a Bézier path in one OpMode

What to tune next

Build docs developers (and LLMs) love

Get Started

Core Concepts

Guides

Documentation Index

​What to tune next

Build docs developers (and LLMs) love

What to tune next