Path planner

Draw the route. Take the Java.

Drag the waypoints to lay out a path. The robot runs it at roughly 40 in/s so you can see where the heading lands, and the code underneath rewrites itself as you move.

x runs right, y runs up the field, heading 0 faces +x and turns counter-clockwise — Pedro's convention, not the SDK's. Squares are 24" tiles. This is geometry only: nothing here knows your motors, your weight, or how much traction you have, so read the timing as a sketch.

0/116 in · x 9.0 · y 60.0 · 0°

Robot

Width inLength in

Defaults to 18 × 18. Change it to your robot.

Click the field to add a point · arrows nudge, shift for 5″ · delete removes · space plays · ⌘Z undoes

Waypoints

namexyheading°

Leg 1 → 2

Curved (BezierCurve)Turn done by 0.80

Import existing Java

Pedro Pathing code

package org.firstinspires.ftc.teamcode.pedroPathing;

import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierCurve;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.PathChain;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;

@Autonomous(name = "GeneratedPath")
public class GeneratedPath extends OpMode {

    private Follower follower;
    private PathChain path;

    private final Pose startPose = new Pose(9, 60, Math.toRadians(0));
    private final Pose scorePose = new Pose(60, 84, Math.toRadians(90));
    private final Pose pickupPose = new Pose(108, 60, Math.toRadians(0));

    @Override
    public void init() {
        follower = Constants.createFollower(hardwareMap);
        follower.setStartingPose(startPose);
        path = follower.pathBuilder()
                .addPath(new BezierLine(startPose, scorePose))
                .setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading(), 0.8)
                .addPath(new BezierCurve(scorePose, new Pose(84, 96), pickupPose))
                .setLinearHeadingInterpolation(scorePose.getHeading(), pickupPose.getHeading(), 0.8)
                .build();
    }

    @Override
    public void start() {
        follower.followPath(path);
    }

    @Override
    public void loop() {
        follower.update();
        // isBusy() goes false once the follower has finished correcting.
        if (!follower.isBusy()) {
            telemetry.addLine("Path complete");
        }
        telemetry.addData("x", follower.getPose().getX());
        telemetry.addData("y", follower.getPose().getY());
        telemetry.addData("heading (deg)", Math.toDegrees(follower.getPose().getHeading()));
        telemetry.update();
    }
}