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();
}

TeleOp with Pedro

@Override
public void loop() {
    follower.update();
    follower.setTeleOpMovementVectors(
        -gamepad1.left_stick_y,   // forward
        -gamepad1.left_stick_x,   // strafe
        -gamepad1.right_stick_x,  // turn
        true                      // robot-centric (false for field-centric)
    );
}

Key Concepts

• Pose: Position (x, y) + heading in radians. Field is 144×144 inches, origin bottom-left
• BezierLine: Straight path between two points
• BezierCurve: Curved path with control points (minimum 3 points)
• PathChain: Multiple path segments as one unit
• Follower: Main controller – call update() every loop cycle

Transition Conditions

Check when path is complete:

• !follower.isBusy() – Path finished
• pathTimer.getElapsedTimeSeconds() > 1.5 – Time elapsed
• follower.getPose().getX() > 36 – Position threshold

Anti-Patterns

Don’t: Forget to call update()

// BAD - Robot won't follow paths
@Override
public void loop() {
    autonomousPathUpdate();  // Missing follower.update()!
}

// GOOD - Always call update() first
@Override
public void loop() {
    follower.update();  // MUST be called every loop
    autonomousPathUpdate();
}

Don’t: Forget to set starting pose

// BAD - Robot thinks it's at (0, 0, 0)
@Override
public void init() {
    follower = Constants.createFollower(hardwareMap);
    buildPaths();  // Paths may be wrong!
}

// GOOD - Set actual starting position
@Override
public void init() {
    follower = Constants.createFollower(hardwareMap);
    follower.setStartingPose(startPose);
    buildPaths();
}

Don’t: Use holdEnd when you don’t need it

// BAD - Robot fights to hold position between paths
follower.followPath(path1, true);  // Holds endpoint
follower.followPath(path2, true);  // Unnecessary hold

// GOOD - Only hold on final path or when needed
follower.followPath(path1);        // No hold between paths
follower.followPath(path2, true);  // Hold at end

Don’t: Use degrees for heading

// BAD - Heading in degrees causes wrong rotation
Pose target = new Pose(24, 48, 90);  // 90 degrees? No!

// GOOD - Always use radians
Pose target = new Pose(24, 48, Math.toRadians(90));

Reference Documentation

• HEADING_INTERPOLATION.md – All heading options
• CONSTRAINTS.md – Path completion constraints
• CONSTANTS_SETUP.md – Robot configuration
• TUNING.md – Tuning process overview