> 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/path-planning-with-pedro-pathing.md).

# Path planning with Pedro Pathing

Path planning lets the robot follow smooth curves through field coordinates while correcting off its live pose.

Pedro Pathing is a reactive path follower built around Bézier curves and a vector-based follower.

### Setup

Install Pedro Pathing with the current instructions at `pedropathing.com`. Then generate a constants file and create a follower.

```java
import com.pedropathing.follower.Follower;

Follower follower = Constants.createFollower(hardwareMap);
follower.setStartingPose(startPose);
```

Tune:

* localizer constants
* follower PIDF constants
* mass and centripetal scaling

### Poses, points, and paths

* A **`Pose`** is `(x, y, heading)`.
* A **`BezierLine`** is a straight path.
* A **`BezierCurve`** bends through control points.
* Heading interpolation controls robot rotation along the path.

```java
import com.pedropathing.pathgen.*;

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 PathChain scorePreload, pickup1;

public void buildPaths() {
    scorePreload = follower.pathBuilder()
            .addPath(new BezierLine(new Point(startPose), new Point(scorePose)))
            .setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading())
            .build();

    pickup1 = follower.pathBuilder()
            .addPath(new BezierLine(new Point(scorePose), new Point(pickupPose)))
            .setLinearHeadingInterpolation(scorePose.getHeading(), pickupPose.getHeading())
            .build();
}
```

### Following paths with a state machine

Use a finite state machine to follow one path at a time and trigger mechanism actions at the right moments.

```java
private int pathState;
private final Timer pathTimer = new Timer();

public void setPathState(int state) {
    pathState = state;
    pathTimer.resetTimer();
}

public void autonomousPathUpdate() {
    switch (pathState) {
        case 0:
            follower.followPath(scorePreload);
            setPathState(1);
            break;

        case 1:
            if (!follower.isBusy()) {
                follower.followPath(pickup1, true);
                setPathState(2);
            }
            break;

        case 2:
            if (!follower.isBusy()) {
                setPathState(-1);
            }
            break;
    }
}
```

Wire it into the main loop:

```java
@Override
public void runOpMode() {
    follower = Constants.createFollower(hardwareMap);
    follower.setStartingPose(startPose);
    buildPaths();

    waitForStart();
    setPathState(0);

    while (opModeIsActive()) {
        follower.update();
        autonomousPathUpdate();

        telemetry.addData("path state", pathState);
        telemetry.addData("x", follower.getPose().getX());
        telemetry.addData("y", follower.getPose().getY());
        telemetry.addData("heading", Math.toDegrees(follower.getPose().getHeading()));
        telemetry.update();
    }
}
```

### Why reactive following matters

Reactive following uses the live pose every loop. If the robot gets bumped or slips, it steers back onto the path instead of blindly finishing in the wrong place.


---

# 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/path-planning-with-pedro-pathing.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.