> For the complete documentation index, see [llms.txt](https://docs.cb21829.org/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://docs.cb21829.org/ftc-java-advanced-guide/reference-examples-complete-programs.md).

# Reference examples (complete programs)

### A PID and gravity-feedforward lift

```java
package org.firstinspires.ftc.teamcode;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;

@Config
@TeleOp(name = "PID Lift", group = "Reference")
public class PidLift extends LinearOpMode {

    public static double kP = 0.01, kI = 0.0, kD = 0.0004, kG = 0.08;
    public static int TARGET_LOW = 0, TARGET_HIGH = 2200;

    @Override
    public void runOpMode() {
        DcMotorEx lift = hardwareMap.get(DcMotorEx.class, "lift");
        lift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        lift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        PIDController pid = new PIDController(kP, kI, kD);
        int target = TARGET_LOW;

        waitForStart();
        while (opModeIsActive()) {
            if (gamepad2.y) target = TARGET_HIGH;
            if (gamepad2.a) target = TARGET_LOW;

            int pos = lift.getCurrentPosition();
            double output = pid.calculate(target, pos) + kG;
            lift.setPower(output);

            telemetry.addData("target", target);
            telemetry.addData("pos", pos);
            telemetry.addData("output", output);
            telemetry.update();
        }
    }
}
```

### A velocity-controlled flywheel

```java
package org.firstinspires.ftc.teamcode;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;

@Config
@TeleOp(name = "Flywheel Velocity", group = "Reference")
public class Flywheel extends LinearOpMode {

    public static double TARGET_TPS = 1800;
    public static double kP = 20, kI = 0, kD = 0, kF = 14;

    @Override
    public void runOpMode() {
        DcMotorEx shooter = hardwareMap.get(DcMotorEx.class, "shooter");
        shooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        shooter.setVelocityPIDFCoefficients(kP, kI, kD, kF);

        waitForStart();
        while (opModeIsActive()) {
            double target = gamepad1.right_trigger > 0.1 ? TARGET_TPS : 0;
            shooter.setVelocity(target);

            double actual = shooter.getVelocity();
            boolean atSpeed = Math.abs(actual - target) < 50;

            telemetry.addData("target tps", target);
            telemetry.addData("actual tps", actual);
            telemetry.addData("AT SPEED", atSpeed);
            telemetry.update();
        }
    }
}
```

### A SquID controller

```java
public class SquIDController {
    private final double kP;

    public SquIDController(double kP) { this.kP = kP; }

    public double calculate(double target, double state) {
        double error = target - state;
        return kP * Math.signum(error) * Math.sqrt(Math.abs(error));
    }
}
```

### A motion-profiled linear slide

```java
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.ElapsedTime;

@TeleOp(name = "Profiled Slide", group = "Reference")
public class ProfiledSlide extends LinearOpMode {

    private static final double MAX_VEL   = 2000;
    private static final double MAX_ACCEL = 4000;

    @Override
    public void runOpMode() {
        DcMotorEx slide = hardwareMap.get(DcMotorEx.class, "slide");
        slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        slide.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

        PIDController pid = new PIDController(0.01, 0.0, 0.0003);
        ElapsedTime profileTimer = new ElapsedTime();

        double startPos = 0;
        double goalPos  = 0;

        waitForStart();
        while (opModeIsActive()) {
            if (gamepad2.y) { startPos = slide.getCurrentPosition(); goalPos = 2200; profileTimer.reset(); }
            if (gamepad2.a) { startPos = slide.getCurrentPosition(); goalPos = 0;    profileTimer.reset(); }

            double distance = goalPos - startPos;
            double dir = Math.signum(distance);
            double profiled = motionProfilePosition(
                    MAX_ACCEL, MAX_VEL, Math.abs(distance), profileTimer.seconds());
            double targetPos = startPos + dir * profiled;

            double power = pid.calculate(targetPos, slide.getCurrentPosition());
            slide.setPower(power);

            telemetry.addData("profiled target", targetPos);
            telemetry.addData("actual", slide.getCurrentPosition());
            telemetry.update();
        }
    }

    public static double motionProfilePosition(double maxAccel, double maxVel,
                                               double distance, double elapsedTime) {
        double accelTime = maxVel / maxAccel;
        double accelDist = 0.5 * maxAccel * accelTime * accelTime;
        if (accelDist > distance / 2.0) {
            accelTime = Math.sqrt(distance / maxAccel);
            accelDist = distance / 2.0;
        }
        double cruiseDist = distance - 2 * accelDist;
        double cruiseTime = cruiseDist / maxVel;
        double totalTime  = 2 * accelTime + cruiseTime;
        double t = Math.min(elapsedTime, totalTime);
        if (t < accelTime) {
            return 0.5 * maxAccel * t * t;
        } else if (t < accelTime + cruiseTime) {
            return accelDist + maxVel * (t - accelTime);
        } else {
            double dt = t - accelTime - cruiseTime;
            return accelDist + cruiseDist + maxVel * dt - 0.5 * maxAccel * dt * dt;
        }
    }
}
```

### A complete Pedro Pathing autonomous

```java
package org.firstinspires.ftc.teamcode;

import com.pedropathing.follower.Follower;
import com.pedropathing.localization.Pose;
import com.pedropathing.pathgen.BezierLine;
import com.pedropathing.pathgen.Point;
import com.pedropathing.util.Timer;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;

@Autonomous(name = "Pedro Auto", group = "Reference")
public class PedroAuto extends LinearOpMode {

    private Follower follower;
    private DcMotor lift;
    private Timer pathTimer;
    private int pathState;

    private final Pose startPose  = new Pose(9,  60, Math.toRadians(0));
    private final Pose scorePose  = new Pose(37, 72, Math.toRadians(0));
    private final Pose pickupPose = new Pose(24, 120, Math.toRadians(90));

    private com.pedropathing.pathgen.PathChain toScore, toPickup;

    @Override
    public void runOpMode() {
        follower = Constants.createFollower(hardwareMap);
        follower.setStartingPose(startPose);
        lift = hardwareMap.get(DcMotor.class, "lift");
        pathTimer = new Timer();

        buildPaths();

        waitForStart();
        setPathState(0);

        while (opModeIsActive()) {
            follower.update();
            autoUpdate();
            telemetry.addData("state", pathState);
            telemetry.addData("x", follower.getPose().getX());
            telemetry.addData("y", follower.getPose().getY());
            telemetry.update();
        }
    }

    private void buildPaths() {
        toScore = follower.pathBuilder()
                .addPath(new BezierLine(new Point(startPose), new Point(scorePose)))
                .setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading())
                .build();
        toPickup = follower.pathBuilder()
                .addPath(new BezierLine(new Point(scorePose), new Point(pickupPose)))
                .setLinearHeadingInterpolation(scorePose.getHeading(), pickupPose.getHeading())
                .build();
    }

    private void autoUpdate() {
        switch (pathState) {
            case 0:
                follower.followPath(toScore);
                setPathState(1);
                break;
            case 1:
                if (!follower.isBusy()) {
                    lift.setPower(0.8);
                    if (pathTimer.getElapsedTimeSeconds() > 1.0) {
                        lift.setPower(0);
                        follower.followPath(toPickup, true);
                        setPathState(2);
                    }
                }
                break;
            case 2:
                if (!follower.isBusy()) setPathState(-1);
                break;
        }
    }

    private void setPathState(int s) { pathState = s; pathTimer.resetTimer(); }
}
```

### A complete FSM autonomous

```java
package org.firstinspires.ftc.teamcode;

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.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;

@Autonomous(name = "FSM Auto", group = "Reference")
public class FsmAuto extends LinearOpMode {

    private enum State { DRIVE_OUT, RAISE_LIFT, DROP, RETRACT, DONE }

    @Override
    public void runOpMode() {
        DcMotor drive = hardwareMap.get(DcMotor.class, "frontLeft");
        DcMotor lift  = hardwareMap.get(DcMotor.class, "lift");
        Servo dumper  = hardwareMap.get(Servo.class,   "dumper");

        State state = State.DRIVE_OUT;
        ElapsedTime timer = new ElapsedTime();

        waitForStart();
        drive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        timer.reset();

        while (opModeIsActive() && state != State.DONE) {
            switch (state) {
                case DRIVE_OUT:
                    drive.setPower(0.5);
                    if (drive.getCurrentPosition() > 1000) {
                        drive.setPower(0);
                        lift.setTargetPosition(2200);
                        lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        lift.setPower(0.8);
                        state = State.RAISE_LIFT;
                    }
                    break;
                case RAISE_LIFT:
                    if (!lift.isBusy()) { dumper.setPosition(0.9); timer.reset(); state = State.DROP; }
                    break;
                case DROP:
                    if (timer.seconds() > 1.0) {
                        dumper.setPosition(0.1);
                        lift.setTargetPosition(0);
                        state = State.RETRACT;
                    }
                    break;
                case RETRACT:
                    if (!lift.isBusy()) state = State.DONE;
                    break;
            }
            telemetry.addData("state", state);
            telemetry.update();
        }
    }
}
```

### A complete NextFTC subsystem and command TeleOp

```java
package org.firstinspires.ftc.teamcode.subsystems;

import com.rowanmcalpin.nextftc.core.Subsystem;
import com.rowanmcalpin.nextftc.core.command.Command;
import com.rowanmcalpin.nextftc.ftc.hardware.controllables.MotorEx;
import com.rowanmcalpin.nextftc.core.control.controllers.PIDFController;
import com.rowanmcalpin.nextftc.ftc.hardware.controllables.RunToPosition;

public class Lift extends Subsystem {
    public static final Lift INSTANCE = new Lift();
    private Lift() { }

    private MotorEx motor;
    private final PIDFController controller = new PIDFController(0.005, 0, 0, 0);

    @Override public void initialize() { motor = new MotorEx("lift"); }

    public Command toHigh() { return new RunToPosition(motor, 2200, controller, this); }
    public Command toLow()  { return new RunToPosition(motor, 0,    controller, this); }
}
```

```java
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.rowanmcalpin.nextftc.ftc.NextFTCOpMode;
import com.rowanmcalpin.nextftc.ftc.components.BulkReadComponent;
import com.rowanmcalpin.nextftc.ftc.components.SubsystemComponent;
import org.firstinspires.ftc.teamcode.subsystems.Lift;

@TeleOp(name = "Command TeleOp", group = "Reference")
public class CommandTeleOp extends NextFTCOpMode {

    public CommandTeleOp() {
        addComponents(
            new SubsystemComponent(Lift.INSTANCE),
            BulkReadComponent.INSTANCE
        );
    }

    @Override
    public void onStartButtonPressed() {
        gamepadManager.getGamepad2().getA().setPressedCommand(Lift.INSTANCE::toHigh);
        gamepadManager.getGamepad2().getB().setPressedCommand(Lift.INSTANCE::toLow);
    }
}
```

### A complete AprilTag alignment routine

```java
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;

@Autonomous(name = "AprilTag Align", group = "Reference")
public class AprilTagAlign extends LinearOpMode {

    private static final int TARGET_ID = 5;
    private static final double DESIRED_RANGE = 12.0;
    private static final double kRange = 0.02, kBearing = 0.015, kYaw = 0.01;

    @Override
    public void runOpMode() {
        DcMotor frontLeft  = hardwareMap.get(DcMotor.class, "frontLeft");
        DcMotor backLeft   = hardwareMap.get(DcMotor.class, "backLeft");
        DcMotor frontRight = hardwareMap.get(DcMotor.class, "frontRight");
        DcMotor backRight  = hardwareMap.get(DcMotor.class, "backRight");
        frontLeft.setDirection(DcMotor.Direction.REVERSE);
        backLeft.setDirection(DcMotor.Direction.REVERSE);

        AprilTagProcessor tagProcessor = AprilTagProcessor.easyCreateWithDefaults();
        VisionPortal portal = new VisionPortal.Builder()
                .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
                .addProcessor(tagProcessor)
                .build();

        waitForStart();
        while (opModeIsActive()) {
            AprilTagDetection tag = null;
            for (AprilTagDetection d : tagProcessor.getDetections()) {
                if (d.metadata != null && d.id == TARGET_ID) { tag = d; break; }
            }

            if (tag != null) {
                double rangeErr   = tag.ftcPose.range - DESIRED_RANGE;
                double bearingErr = tag.ftcPose.bearing;
                double yawErr     = tag.ftcPose.yaw;

                double drive  = -kRange   * rangeErr;
                double turn   =  kBearing * bearingErr;
                double strafe =  kYaw     * yawErr;

                double denom = Math.max(Math.abs(drive) + Math.abs(strafe) + Math.abs(turn), 1.0);
                frontLeft.setPower((drive + strafe + turn) / denom);
                backLeft.setPower((drive - strafe + turn) / denom);
                frontRight.setPower((drive - strafe - turn) / denom);
                backRight.setPower((drive + strafe - turn) / denom);

                telemetry.addData("range", tag.ftcPose.range);
                telemetry.addData("bearing", tag.ftcPose.bearing);
            } else {
                frontLeft.setPower(0); backLeft.setPower(0);
                frontRight.setPower(0); backRight.setPower(0);
                telemetry.addLine("Searching for tag " + TARGET_ID);
            }
            telemetry.update();
        }
    }
}
```

### A complete EasyOpenCV detection OpMode

```java
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import org.openftc.easyopencv.*;

@Autonomous(name = "Vision Auto", group = "Reference")
public class VisionAuto extends LinearOpMode {

    @Override
    public void runOpMode() {
        int viewId = hardwareMap.appContext.getResources().getIdentifier(
                "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
        OpenCvCamera camera = OpenCvCameraFactory.getInstance().createWebcam(
                hardwareMap.get(WebcamName.class, "Webcam 1"), viewId);

        ZonePipeline pipeline = new ZonePipeline();
        camera.setPipeline(pipeline);
        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
            @Override public void onOpened() { camera.startStreaming(640, 480, OpenCvCameraRotation.UPRIGHT); }
            @Override public void onError(int code) { }
        });

        while (opModeInInit()) {
            telemetry.addData("Detected zone", pipeline.getZone());
            telemetry.update();
        }

        waitForStart();
        camera.stopStreaming();

        switch (pipeline.getZone()) {
            case "LEFT":   break;
            case "CENTER": break;
            default:        break;
        }
    }

    static class ZonePipeline extends OpenCvPipeline {
        private volatile String zone = "RIGHT";

        @Override
        public Mat processFrame(Mat input) {
            Mat hsv = new Mat();
            Imgproc.cvtColor(input, hsv, Imgproc.COLOR_RGB2HSV);
            Mat mask = new Mat();
            Core.inRange(hsv, new Scalar(0, 100, 100), new Scalar(15, 255, 255), mask);

            int w = input.width();
            double left   = Core.sumElems(mask.submat(new Rect(0,     0, w/3, input.height()))).val[0];
            double center = Core.sumElems(mask.submat(new Rect(w/3,   0, w/3, input.height()))).val[0];
            double right  = Core.sumElems(mask.submat(new Rect(2*w/3, 0, w/3, input.height()))).val[0];

            if (left > center && left > right)        zone = "LEFT";
            else if (center > left && center > right) zone = "CENTER";
            else                                      zone = "RIGHT";

            hsv.release();
            mask.release();
            return input;
        }

        public String getZone() { return zone; }
    }
}
```

### A loop-time profiler and bulk-read scaffold

```java
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.util.ElapsedTime;
import java.util.List;

List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
for (LynxModule hub : allHubs) {
    hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
ElapsedTime loopTimer = new ElapsedTime();

double loopMs = loopTimer.milliseconds();
loopTimer.reset();
telemetry.addData("loop (ms)", String.format("%.1f", loopMs));
telemetry.addData("hz", String.format("%.0f", 1000.0 / loopMs));
telemetry.update();
```


---

# Agent Instructions
This documentation is published with GitBook. GitBook is the documentation platform designed so that both humans and AI agents can read, navigate, and reason over technical content effectively. Learn more at gitbook.com.

## Querying This Documentation
If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.

Perform an HTTP GET request on the current page URL with the `ask` query parameter, and the optional `goal` query parameter:

```
GET https://docs.cb21829.org/ftc-java-advanced-guide/reference-examples-complete-programs.md?ask=<question>&goal=<endgoal>
```

`ask` is the immediate question: it should be specific, self-contained, and written in natural language.
`goal` is optional and describes the broader end goal you are ultimately trying to accomplish on behalf of the user. GitBook uses it to tailor the answer towards what is most useful for that goal.

The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.

Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.