# Pedro Pathing Library

## Introduction

Pedro Pathing is a comprehensive autonomous path following library designed for FTC (FIRST Tech Challenge) robotics. The library provides advanced path planning and following capabilities using Bézier curves for smooth trajectory generation. It enables robots to autonomously navigate complex paths with precise position and heading control through sophisticated PIDF (Proportional-Integral-Derivative-Feedforward) controllers and multiple localization methods including three-wheel odometry, two-wheel odometry, drive encoders, and specialized sensors like the SparkFun OTOS and goBILDA Pinpoint.

The library implements a follower-based architecture that continuously tracks the robot's pose, calculates errors from the desired path, and generates corrective motor commands. It supports path chaining for sequential autonomous routines, callback mechanisms for executing actions during path following, and flexible heading interpolation strategies. Pedro Pathing abstracts the complexity of autonomous motion control while providing extensive customization options for tuning robot behavior through constraints, PIDF coefficients, and deceleration profiles.

## Core APIs and Functions

### Creating and Initializing a Follower

The Follower is the main control class that manages path following, localization, and motor control.

```java
// Using FollowerBuilder for easy setup
FollowerBuilder builder = new FollowerBuilder(followerConstants, hardwareMap);

Follower follower = builder
    .threeWheelLocalizer(threeWheelConstants)  // Set localization method
    .mecanumDrivetrain(mecanumConstants)        // Set drivetrain type
    .pathConstraints(PathConstraints.defaultConstraints)  // Set constraints
    .build();

// Set starting position
follower.setStartingPose(new Pose(0, 0, 0));

// In your loop
follower.update();  // Call this every loop iteration
```

### Building Paths with PathBuilder

PathBuilder provides a fluent API for constructing complex path chains with heading control.

```java
// Create a path chain with multiple segments
PathChain pathChain = follower.pathBuilder()
    .addPath(new BezierCurve(
        new Pose(0, 0, 0),
        new Pose(20, 10, 0),
        new Pose(40, 0, 0)
    ))
    .setLinearHeadingInterpolation(0, Math.PI / 2)  // Turn while moving
    .addPath(new BezierCurve(
        new Pose(40, 0, 0),
        new Pose(50, -20, 0),
        new Pose(60, -30, 0)
    ))
    .setConstantHeadingInterpolation(Math.PI / 2)  // Maintain heading
    .addParametricCallback(0.5, () -> {
        // Execute action at 50% of path completion
        System.out.println("Halfway through second path");
    })
    .setBrakingStrength(0.8)  // Set deceleration
    .setVelocityConstraint(5.0)  // End velocity threshold
    .build();

// Follow the path chain
follower.followPath(pathChain, true);  // true = hold position at end

// Check if busy
while (follower.isBusy()) {
    follower.update();
}
```

### Pose Representation and Manipulation

Pose objects represent robot position (x, y) and heading in field coordinates.

```java
// Create poses with different constructors
Pose origin = new Pose();  // (0, 0, 0)
Pose position = new Pose(24, 36);  // x=24, y=36, heading=0
Pose fullPose = new Pose(24, 36, Math.toRadians(45));  // with heading

// Pose arithmetic
Pose moved = position.plus(new Pose(12, 6, 0));  // Add poses
Pose difference = moved.minus(position);  // Subtract poses
Pose scaled = position.times(2.0);  // Scale by factor

// Utility functions
double distance = position.distanceFrom(origin);  // Euclidean distance
Pose rotated = position.rotate(Math.PI / 4, true);  // Rotate pose
Pose mirrored = position.mirror();  // Mirror across field center

// Get components
double x = fullPose.getX();
double y = fullPose.getY();
double heading = fullPose.getHeading();  // in radians
Vector posVector = fullPose.getAsVector();  // Position as vector
```

### Following Individual Paths

Execute single paths with automatic or manual hold at the end.

```java
// Create a simple curved path
BezierCurve curve = new BezierCurve(
    new Pose(0, 0, 0),
    new Pose(24, 12, 0),
    new Pose(48, 0, 0)
);
Path path = new Path(curve);

// Set heading interpolation
path.setLinearHeadingInterpolation(0, Math.PI, 0.8);  // Interpolate to t=0.8

// Set path constraints
path.setVelocityConstraint(2.0);  // Max 2 in/s at end
path.setTranslationalConstraint(1.0);  // Within 1 inch
path.setHeadingConstraint(Math.toRadians(5));  // Within 5 degrees

// Follow the path
follower.followPath(path, true);  // Hold at end
while (follower.isBusy()) {
    follower.update();

    // Get path progress
    double completion = follower.getPathCompletion();  // 0.0 to 1.0
    double distanceRemaining = follower.getDistanceRemaining();

    // Check status
    if (follower.atParametricEnd()) {
        System.out.println("Reached parametric end of path");
    }
}
```

### Holding Position

Command the robot to hold a specific position and heading.

```java
// Hold current position
follower.holdPoint(follower.getPose());

// Hold specific position with heading
Pose targetHold = new Pose(48, 24, Math.toRadians(90));
follower.holdPoint(targetHold);

// Hold using BezierPoint (more control)
BezierPoint point = new BezierPoint(new Pose(48, 24));
follower.holdPoint(point, Math.toRadians(90));

// Hold without scaling (full power corrections)
follower.holdPoint(point, Math.toRadians(90), false);

// Check if at target position
if (follower.atPose(targetHold, 1.0, 1.0, Math.toRadians(2))) {
    System.out.println("Within tolerance");
}
```

### Turning in Place

Execute precise turns without translational movement.

```java
// Turn to absolute heading
follower.turnTo(Math.toRadians(90));  // Face 90 degrees

// Turn to heading in degrees
follower.turnToDegrees(90);

// Relative turns
follower.turn(Math.toRadians(45), true);  // Turn 45 deg left
follower.turn(Math.toRadians(45), false);  // Turn 45 deg right

// Relative turns in degrees
follower.turnDegrees(90, true);  // Turn 90 degrees left

// Wait for turn to complete
while (follower.isTurning()) {
    follower.update();
    double headingError = follower.getHeadingError();
}
```

### Path Callbacks for Sequential Actions

Execute code at specific points during path following without blocking.

```java
PathChain pathWithCallbacks = follower.pathBuilder()
    .addPath(curve1)
    .addTemporalCallback(1000, () -> {
        // Run 1000ms after path starts
        robot.intake.deploy();
    })
    .addParametricCallback(0.7, () -> {
        // Run at 70% path completion
        robot.intake.start();
    })
    .addPath(curve2)
    .addPoseCallback(
        new Pose(48, 24),  // Target position
        () -> robot.lift.raise(),  // Action to execute
        0.5  // Initial t-value guess for optimization
    )
    .addCallback(() -> {
        // Custom condition-based callback
        return robot.sensor.hasGameElement();
    }, () -> {
        robot.intake.stop();
    })
    .build();

follower.followPath(pathWithCallbacks);
```

### Teleop Drive Control

Switch between autonomous path following and manual driver control.

```java
// Enable teleop drive mode
follower.startTeleopDrive();  // Default brake mode
follower.startTeleopDrive(true);  // Explicit brake mode
follower.startTeleopDrive(false);  // Coast mode

// In teleop loop
double forward = -gamepad1.left_stick_y;
double strafe = gamepad1.left_stick_x;
double turn = gamepad1.right_stick_x;

// Robot-centric control
follower.setTeleOpDrive(forward, strafe, turn);

// Field-centric control with offset
follower.setTeleOpDrive(forward, strafe, turn, Math.toRadians(0));

// Field-centric with explicit flag
follower.setTeleOpDrive(forward, strafe, turn, false, Math.toRadians(0));

// Update in loop
follower.update();

// Check mode
if (follower.isTeleopDrive()) {
    // Currently in teleop mode
}
```

### Configuring PIDF Controllers

Tune the control system for optimal path following performance.

```java
// Set drive PIDF coefficients (forward correction)
FilteredPIDFCoefficients driveCoeffs = new FilteredPIDFCoefficients(
    0.1,   // kP - proportional gain
    0.0,   // kI - integral gain
    0.01,  // kD - derivative gain
    0.0    // kF - feedforward gain
);
follower.setDrivePIDFCoefficients(driveCoeffs);

// Set heading PIDF coefficients (rotation correction)
PIDFCoefficients headingCoeffs = new PIDFCoefficients(
    2.0,   // kP
    0.0,   // kI
    0.1,   // kD
    0.0    // kF
);
follower.setHeadingPIDFCoefficients(headingCoeffs);

// Set translational PIDF (lateral correction)
PIDFCoefficients transCoeffs = new PIDFCoefficients(0.15, 0.0, 0.01, 0.0);
follower.setTranslationalPIDFCoefficients(transCoeffs);

// Set secondary coefficients for different path phases
follower.setSecondaryDrivePIDFCoefficients(secondaryDriveCoeffs);
follower.setSecondaryHeadingPIDFCoefficients(secondaryHeadingCoeffs);
```

### Localization Setup and Pose Tracking

Configure different localization methods for accurate robot tracking.

```java
// Three-wheel odometry localizer
ThreeWheelConstants threeWheelConsts = new ThreeWheelConstants();
threeWheelConsts.forwardTicksToInches = 0.001989436789;
threeWheelConsts.strafeTicksToInches = 0.001989436789;
threeWheelConsts.turnTicksToInches = 0.001989436789;
threeWheelConsts.leftPodY = -5.5;
threeWheelConsts.rightPodY = 5.5;
threeWheelConsts.strafePodX = -3.0;
threeWheelConsts.leftEncoder_HardwareMapName = "leftFront";
threeWheelConsts.rightEncoder_HardwareMapName = "rightFront";
threeWheelConsts.strafeEncoder_HardwareMapName = "leftRear";

ThreeWheelLocalizer localizer = new ThreeWheelLocalizer(hardwareMap, threeWheelConsts);

// Or use FollowerBuilder
Follower follower = new FollowerBuilder(followerConstants, hardwareMap)
    .threeWheelLocalizer(threeWheelConsts)
    .mecanumDrivetrain(mecanumConstants)
    .build();

// Get pose and velocity
Pose currentPose = follower.getPose();
Vector velocity = follower.getVelocity();
double angularVel = follower.getAngularVelocity();
double totalHeading = follower.getTotalHeading();  // Unwrapped heading

// Manual pose updates
follower.setPose(new Pose(24, 36, 0));  // Relocalize robot
```

### Path Constraints Configuration

Define kinematic and dynamic constraints for path following behavior.

```java
PathConstraints constraints = new PathConstraints();

// Velocity and acceleration
constraints.setMaxVelocity(40.0);  // inches per second
constraints.setMaxAcceleration(30.0);  // inches per second²
constraints.setMaxAngularVelocity(Math.toRadians(180));  // radians per second

// Deceleration parameters
constraints.setBrakingStrength(0.7);  // Multiplier for deceleration
constraints.setBrakingStart(0.8);  // Start deceleration at 80% of path

// End constraints
constraints.setVelocityConstraint(3.0);  // Max velocity at path end
constraints.setTranslationalConstraint(0.5);  // Position error tolerance
constraints.setHeadingConstraint(Math.toRadians(3));  // Heading error tolerance
constraints.setTValueConstraint(0.95);  // Parametric completion threshold
constraints.setTimeoutConstraint(500);  // Max correction time (ms)

// Search parameters
constraints.setBEZIER_CURVE_SEARCH_LIMIT(10);  // Binary search iterations

// Apply to follower or paths
PathConstraints.setDefaultConstraints(constraints);
follower.setConstraints(constraints);
```

### Advanced PathBuilder Features

Use sophisticated path generation with automatic curve fitting.

```java
// Automatically generate smooth curves through points
PathChain smoothPath = follower.pathBuilder()
    .curveThrough(
        0.6,  // Tension parameter (0.0-1.0)
        new Pose(0, 0, 0),
        new Pose(24, 12, 0),
        new Pose(48, 8, 0),
        new Pose(72, -4, 0),
        new Pose(96, 0, 0)
    )
    .setGlobalLinearHeadingInterpolation(0, Math.PI)  // Apply to all paths
    .setGlobalDeceleration()  // Decelerate across entire chain
    .build();

// Piecewise heading interpolation
PathChain complexHeading = follower.pathBuilder()
    .addPath(curve)
    .setLinearHeadingInterpolation(
        0,  // Start heading
        Math.PI / 2,  // End heading
        0.8,  // End time
        0.2  // Start time
    )
    .build();

// Set reversed heading (face backwards while driving forward)
PathChain reversed = follower.pathBuilder()
    .addPath(curve)
    .setReversed()
    .build();

// Custom heading interpolation
PathChain customHeading = follower.pathBuilder()
    .addPath(curve)
    .setHeadingInterpolation(pathPoint -> {
        // Custom function: heading as function of t
        return Math.sin(pathPoint.tValue * Math.PI);
    })
    .build();
```

### Error Monitoring and Debugging

Access real-time error values and diagnostic information.

```java
// Get current errors
double headingError = follower.getHeadingError();  // Radians
Vector translationalError = follower.getTranslationalError();  // Position error
double driveError = follower.getDriveError();  // Forward path error

// Get correction vectors
Vector driveVector = follower.getDriveVector();  // Forward correction
Vector correctiveVector = follower.getCorrectiveVector();  // Lateral correction
Vector headingVector = follower.getHeadingVector();  // Rotation correction
Vector centripetalCorrection = follower.getCentripetalForceCorrection();

// Path progress information
double tValue = follower.getCurrentTValue();  // Parametric position (0-1)
int pathNumber = (int) follower.getCurrentPathNumber();  // In path chain
double traveled = follower.getDistanceTraveledOnPath();
double remaining = follower.getDistanceRemaining();

// Debug output
String[] debugInfo = follower.debug();
System.out.println("Pose: " + debugInfo[0]);
System.out.println("Error: " + debugInfo[1]);
System.out.println("Vector: " + debugInfo[2]);
System.out.println("Drivetrain: " + debugInfo[3]);

// Check for issues
if (follower.isRobotStuck()) {
    System.out.println("Robot is stuck - zero velocity detected");
}
if (follower.isLocalizationNAN()) {
    System.out.println("Localization error - NaN values detected");
}
```

### Pausing and Resuming Path Following

Temporarily interrupt path following and resume later.

```java
// Follow a path
follower.followPath(pathChain);

while (follower.isBusy()) {
    follower.update();

    // Pause if condition met
    if (gamepad1.a) {
        follower.pausePathFollowing();  // Holds current position
    }
}

// Later, resume from where it left off
if (gamepad1.b && !follower.isBusy()) {
    follower.resumePathFollowing();  // Continue path
}

while (follower.isBusy()) {
    follower.update();
}
```

### Power and Velocity Control

Dynamically adjust robot speed during path following.

```java
// Set maximum power (0.0 to 1.0)
follower.setMaxPower(0.7);  // Limit to 70% power

// Get current power scaling
double currentMaxPower = follower.getMaxPowerScaling();

// Set centripetal scaling for curves
follower.setCentripetalScaling(1.2);  // Increase curve correction

// Manually set drivetrain velocities
follower.setXVelocity(10.0);  // Set forward velocity
follower.setYVelocity(5.0);   // Set strafe velocity

// Selective PIDF activation (for debugging)
follower.activateAllPIDFs();  // Enable all controllers
follower.deactivateAllPIDFs();  // Disable all controllers

follower.activateDrive();  // Enable only drive PIDF
follower.activateHeading();  // Enable only heading PIDF
follower.activateTranslational();  // Enable only translational PIDF
follower.activateCentripetal();  // Enable only centripetal correction
```

## Summary

Pedro Pathing provides a production-ready autonomous navigation solution for FTC robots with extensive customization capabilities. The library excels at executing complex multi-segment paths with precise control over robot position, heading, and velocity throughout the entire motion. Its callback system enables sophisticated autonomous routines where the robot can perform actions like deploying mechanisms, collecting game elements, or triggering sensors while seamlessly continuing path execution. The flexible architecture supports multiple drivetrain types (currently Mecanum with support for others) and various localization methods ranging from basic dead reckoning to advanced sensor fusion.

Integration into existing FTC robot code is straightforward using the FollowerBuilder pattern, requiring only hardware map configuration and periodic update calls in the main control loop. Teams can start with default constraints and PIDF coefficients, then iteratively tune performance based on their specific robot characteristics. The library handles the mathematical complexity of Bézier curve generation, closest point calculations, and feedback control internally while exposing a clean API for path construction and monitoring. For competitive FTC teams seeking reliable autonomous performance, Pedro Pathing offers a mature, well-documented framework that has been proven in tournament conditions and continues to receive active development and community support.