pedropathing

name	pedropathing
description	Expert guidance for creating FTC robot autonomous paths using pedroPathing v2.0. Use when building paths, configuring PathBuilder, using PathChains, adding callbacks, setting speed controls, working with Bezier curves, or debugging path following issues.

pedroPathing FTC Path Following Skill

This skill provides expert guidance for using pedroPathing v2.0, a professional path-following library for FTC robots using Bezier curves, PIDF control, and centripetal force correction.

Overview

pedroPathing enables smooth, fast, and accurate autonomous navigation for FTC robots with:

• Omnidirectional drivetrains (mecanum, X-drive, swerve)
• Active localization (dead wheels, GoBilda Pinpoint, SparkFun OTOS, or drive encoders)
• Bezier curve-based paths with heading interpolation
• Advanced features: callbacks, speed control, path constraints

Core Concepts

Coordinate System

• X-axis: Increases moving right across field
• Y-axis: Increases moving up on field
• Heading: Radians, counterclockwise positive (0 = right, π/2 = up, π = left, 3π/2 = down)
• Units: Inches
• IMPORTANT: Always use Math.toRadians(degrees) - all headings are in RADIANS

Path Types

• BezierLine: Straight line between two poses
• BezierCurve: Smooth curve through control points
  • Typically use 3 points total (start, control, end)
  • Avoid 3+ control points beyond start/end as it's difficult to track
  • No degenerate paths (duplicate or improperly ordered collinear points)

PathBuilder vs Path vs PathChain

• PathBuilder: Method for combining multiple paths with independent/dependent interpolations
• Path: Single path segment
• PathChain: Result of pathBuilder.build(), contains multiple paths with shared configuration

Basic Path Creation Pattern

// Using follower's pathBuilder
val myPath = follower.pathBuilder()
    .addPath(BezierLine(Pose(0.0, 0.0), Pose(24.0, 0.0)))
    .setLinearHeadingInterpolation(0.0, 0.0)
    .build()

follower.followPath(myPath)

PathBuilder API

Adding Paths

.addPath(path)  // Add a BezierLine or BezierCurve

Heading Interpolation (choose one per path segment)

1. Linear Heading Interpolation

.setLinearHeadingInterpolation(startHeading, endHeading, endTime = 0.8)

• Rotates from start to end heading over the path
• endTime (0.0-1.0): when rotation completes (0.8 works well)
• Too low = oscillations, too high = inaccurate end heading

2. Constant Heading Interpolation

.setConstantHeadingInterpolation(setHeading)

• Maintains fixed heading throughout path
• Rotates to heading first if starting with different heading

3. Tangent Heading Interpolation

.setTangentHeadingInterpolation()

• Robot orientation aligns with path's slope
• Drives tangentially to curve

4. Facing Point Interpolation

HeadingInterpolator.facingPoint()

• Robot orients toward specific coordinate while traversing

5. Piecewise Interpolation

• Combines different strategies across path segments using t-values

6. Custom Interpolation

• Implement HeadingInterpolator interface

Reverse Option

.reversed()  // Call on any interpolator to reverse

Global Interpolation (across entire PathChain)

.setGlobalHeadingInterpolation(HeadingInterpolator)

Path Direction

.setReversed()  // Robot drives backward along path

Deceleration Control

Default Deceleration (only on last path)

.setBrakingStrength(double)  // Higher = abrupt stop, lower = smooth (but risks overshoot)

• Value of 1 ≈ old ZPAM of 4

Global Deceleration (across entire PathChain)

.setGlobalDeceleration(double BrakingStrength)
// or
.setGlobalDeceleration()  // Use default

.setBrakingStart(double)  // When to start braking (higher = earlier)

No Deceleration

.setNoDeceleration()  // Full speed, may overshoot

Path Completion Constraints

.setTValueConstraint(double)  // Parametric completion (0.0-1.0)

Individual Path methods (call on Path object):

path.setTimeoutConstraint(double millis)      // Correction time after stopping
path.setTValueConstraint(double)               // How much of path to follow (0.0-1.0)
path.setVelocityConstraint(double)             // Speed threshold (inches/sec)
path.setTranslationalConstraint(double)        // Max position error
path.setHeadingConstraint(double)              // Max heading error (radians)

All 5 constraints must be met for follower.isBusy() to return false.

Path Callbacks

Parametric Callback (RECOMMENDED - distance-based)

.addParametricCallback(double percent, Runnable action)
// Example:
.addParametricCallback(0.5) {
    // Runs after 50% of path distance traveled
    robot.intake.deploy()
}

Temporal Callback (time-based, less reliable)

.addTemporalCallback(double millis, Runnable action)

Pose Callback (position-based)

.addPoseCallback(Pose pose, Runnable action, double guess)

Custom Callback

.addPathCallback(PathCallback callback)
.addPathCallback(CallbackCondition isReady, Runnable action)

Building

.build()  // Returns PathChain

Follower Methods

Following Paths

follower.followPath(pathChain)
follower.followPath(pathChain, maxPower, holdEnd)  // Per-path speed control
follower.update()  // Call in loop!

Speed Control (3 methods)

1. Global Speed Limit (in Constants)

driveConstants.maxPower(0.8)  // 0.0-1.0, works on mecanum only

2. Per-Path Speed

follower.followPath(pathChain, 0.6, true)  // maxPower, holdEnd

3. Persistent Setting

follower.setMaxPower(0.7)  // Active until changed

Path Completion Detection

Primary method:

!follower.isBusy()  // true when path complete and corrected

Alternative (no waiting for correction):

follower.atParametricEnd() &&
    follower.getHeadingError() < follower.getCurrentPath().getHeadingConstraint()

Troubleshooting stuck paths:

// Check individual criteria:
follower.getVelocity().getMagnitude() < follower.getCurrentPath().getVelocityConstraint()
follower.getAngularVelocity() < 0.055

// Or decrease T-Value/Timeout constraints

Pose Control

follower.setStartingPose(Pose)
follower.setPose(Pose)  // For vision correction
follower.getPose()

Pause/Resume

follower.pausePathFollowing()   // Stop mid-path
follower.resumePathFollowing()  // Continue
follower.breakFollowing()       // Reset to first path, clear callbacks

TeleOp Drive

follower.setTeleOpDrive(
    -gamepad1.left_stick_y,
    -gamepad1.left_stick_x,
    -gamepad1.right_stick_x,
    true  // Robot-centric (false for field-centric)
)

Path Creation Examples

Simple Forward

val forward = follower.pathBuilder()
    .addPath(BezierLine(Pose(0.0, 0.0), Pose(24.0, 0.0)))
    .setLinearHeadingInterpolation(0.0, 0.0)
    .build()

Curved Path with Control Point

val curve = follower.pathBuilder()
    .addPath(
        BezierCurve(
            Pose(0.0, 0.0),        // Start
            Pose(12.0, 6.0),       // Control point
            Pose(24.0, 0.0)        // End
        )
    )
    .setTangentHeadingInterpolation()
    .build()

Multi-Segment PathChain

val pickupCycle = follower.pathBuilder()
    .addPath(BezierLine(scorePose, pickup1Pose))
    .setLinearHeadingInterpolation(scorePose.heading, pickup1Pose.heading)
    .addPath(BezierLine(pickup1Pose, scorePose))
    .setLinearHeadingInterpolation(pickup1Pose.heading, scorePose.heading)
    .build()

Reversed Path

val backward = follower.pathBuilder()
    .addPath(BezierLine(Pose(0.0, 0.0), Pose(24.0, 0.0)))
    .setTangentHeadingInterpolation()
    .setReversed()  // Drive backward
    .build()

Path with Callbacks

val pathWithActions = follower.pathBuilder()
    .addPath(BezierLine(Pose(0.0, 0.0), Pose(40.0, 0.0)))
    .setLinearHeadingInterpolation(0.0, 0.0)
    .addParametricCallback(0.3) { robot.arm.extend() }
    .addParametricCallback(0.8) { robot.claw.open() }
    .build()

Lazy Path Generation

// Path from current position (defined at init, calculated at runtime)
val dynamicPath = follower.pathBuilder()
    .addPath(BezierCurve(
        follower::getPose,  // Lazy: gets current pose when path starts
        Pose(24.0, 24.0)
    ))
    .setTangentHeadingInterpolation()
    .build()

Integration with This Codebase

In Autonomous OpModes

// Create follower
val pathing = Pathing(hardwareMap)
pathing.follower.setStartingPose(Pose(x, y, heading))

// Define paths in init (see FarAuto.kt Paths class pattern)
class Paths(follower: Follower) {
    val Path1: PathChain
    val Path2: PathChain

    init {
        Path1 = follower.pathBuilder()
            .addPath(...)
            .build()
    }
}

// Follow using command
FollowPedroPath(pathing, paths.Path1).schedule()

// In loop
robot.run()  // Runs CommandScheduler which updates follower

With FTCLib Commands

// Sequential paths
SequentialCommandGroup(
    FollowPedroPath(pathing, paths.Path1),
    FollowPedroPath(pathing, paths.Path2)
).schedule()

// Parallel with actions
ParallelRaceGroup(
    FollowPedroPath(pathing, paths.Path1),
    RunIntake(robot.intake),
    AutoIndex(robot.spiceRack)
).schedule()

Constants Configuration

Edit pedroPathing/Constants.java:

// FollowerConstants
.mass(23)  // Robot mass in lbs

// PathConstraints
new PathConstraints(
    0.99,  // TValue constraint
    100,   // Timeout constraint
    1,     // Velocity constraint
    1      // Translational constraint
)

// MecanumConstants
.maxPower(1)  // Global speed limit
.rightFrontMotorName("driveFrontRight")
// ... motor names and directions

// TwoWheelConstants (current config)
.forwardEncoder_HardwareMapName("driveFrontRight")
.strafeEncoder_HardwareMapName("driveBackLeft")
.forwardPodY(-2.4)  // Inches from center
.strafePodX(4.5)
.forwardTicksToInches(0.0019370678275907824)
.strafeTicksToInches(0.0019649286280731)

Vision Integration (AprilTags)

// Update pose from vision
follower.setPose(getRobotPoseFromCamera())

// Convert FTC coordinates to Pedro
val ftcPose = Pose2D(...)
val ftcStandard = PoseConverter.pose2DToPose(ftcPose, InvertedFTCCoordinates.INSTANCE)
val pedroPose = ftcStandard.getAsCoordinateSystem(PedroCoordinates.INSTANCE)

Use KalmanFilter or LowPassFilter for smoothing camera data.

Dashboard & Visualization

Panels Dashboard: 192.168.43.1:8001

• Live tuning of constants
• Field visualization
• Real-time logging/graphing
• Pre-installed in this project

Path Visualizer: https://visualizer.pedropathing.com/

Common Patterns for This Codebase

Intake While Driving

ParallelRaceGroup(
    FollowPedroPath(pathing, paths.toSample),
    RunIntake(robot.intake),
    AutoIndex(robot.spiceRack)
)

Shoot After Path

SequentialCommandGroup(
    FollowPedroPath(pathing, paths.toBasket),
    ShootMotif(robot.spiceRack, motif)
).raceWith(RunShooter(robot.shooter))

Field-Relative Paths

Use Pinpoint odometry pose to create paths from current position:

val currentPose = Pose(
    robot.hardware.pinpointPos().x * 39.37,  // Convert meters to inches
    robot.hardware.pinpointPos().y * 39.37,
    robot.hardware.pinpointPos().heading
)

Troubleshooting

Robot doesn't follow path smoothly

• Tune PIDF values in Constants
• Check localization accuracy
• Reduce maxPower
• Simplify Bezier curves (fewer control points)

Path never completes (stuck at end)

• Decrease timeout constraint
• Decrease T-value constraint
• Relax heading/translational constraints
• Check secondaryPIDF is tuned

Robot overshoots

• Increase braking strength
• Start braking earlier (setBrakingStart)
• Reduce velocity constraint

Heading alignment off

• Tune heading PID
• Adjust endTime in linear heading interpolation
• Check IMU calibration

Path seems jerky

• Increase number of Bezier control points
• Smooth transitions between paths
• Check for abrupt heading changes

Resources

• Official Docs: https://pedropathing.com/docs/pathing
• GitHub: https://github.com/Pedro-Pathing/PedroPathing
• Discord: https://discord.gg/2GfC4qBP5s
• Visualizer: https://visualizer.pedropathing.com/
• This Project's Config: TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
• Example Auto: TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/FarAuto.kt

Quick Reference

Task	Method
Create path	follower.pathBuilder().addPath(...).build()
Follow path	follower.followPath(pathChain)
Check if done	!follower.isBusy()
Update (REQUIRED)	follower.update() in loop
Set speed	.followPath(path, 0.8, true)
Add callback	.addParametricCallback(0.5) { action }
Set heading	.setLinearHeadingInterpolation(start, end)
Reverse path	.setReversed()
Control braking	.setBrakingStrength(1.0)
Get pose	follower.getPose()
TeleOp drive	follower.setTeleOpDrive(y, x, turn, robotCentric)

Remember: All headings in RADIANS. Use Math.toRadians(degrees).