pedro-pathing

Pedro Pathing

Pedro Pathing is a path follower using Bezier curves for smooth autonomous navigation.

Quick Start

Autonomous OpMode Structure

@Autonomous(name = "My Auto")
public class MyAuto extends OpMode {
    private Follower follower;
    private Timer pathTimer;
    private int pathState;

    // Define poses (x, y, heading in radians)
    private final Pose startPose = new Pose(7, 6.75, Math.toRadians(0));
    private final Pose targetPose = new Pose(24, 48, Math.toRadians(90));

    @Override
    public void init() {
        pathTimer = new Timer();
        follower = Constants.createFollower(hardwareMap);
        buildPaths();
        follower.setStartingPose(startPose);
    }

    @Override
    public void loop() {
        follower.update();  // MUST call every loop
        autonomousPathUpdate();
        telemetry.update();
    }
}

Building Paths

// Straight line path
Path straightPath = new Path(new BezierLine(startPose, endPose));
straightPath.setLinearHeadingInterpolation(startPose.getHeading(), endPose.getHeading());

// PathChain (multiple segments or single path via builder)
PathChain myChain = follower.pathBuilder()
    .addPath(new BezierLine(startPose, endPose))
    .setLinearHeadingInterpolation(startPose.getHeading(), endPose.getHeading())
    .build();

Following Paths

follower.followPath(myPath);           // Follow without holding endpoint
follower.followPath(myChain, true);    // Hold endpoint position when done

State Machine Pattern

public void autonomousPathUpdate() {
    switch (pathState) {
        case 0:
            follower.followPath(firstPath);
            setPathState(1);
            break;
        case 1:
            if (!follower.isBusy()) {
                // Path complete, do action then next path
                follower.followPath(nextPath, true);
                setPathState(2);
            }
            break;
    }
}

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