Heading Interpolation

Advanced control over robot rotation during path following

What is Interpolation?

Interpolation determines how the robot's heading (rotation) changes as it moves along a path. Different strategies allow you to optimize robot orientation for specific game tasks like intaking, scoring, or avoiding obstacles.

Constant Heading Interpolation

Maintains a fixed heading throughout the entire path segment, regardless of direction of travel.

Usage:

Path path = new BezierLine(new Point(48, 24, Point.CARTESIAN))
    .setConstantHeadingInterpolation(Math.toRadians(45));

// Robot will maintain 45° heading even while moving in different directions

Best Use Cases:

Intake Operations: Keep intake facing game elements while strafing
Vision Tracking: Maintain camera orientation toward target
Defensive Positioning: Face opponent while repositioning
Precise Alignment: Stay aligned with scoring elements during approach

Example: Strafe While Facing Forward

// Robot strafes left 24 inches but always faces forward (0°)
follower.followPath(
    new BezierLine(new Point(0, 24, Point.CARTESIAN))
        .setConstantHeadingInterpolation(Math.toRadians(0))
);

// Useful for keeping intake pointed at game elements while positioning

⚠️ Performance Note

Constant heading requires the robot to strafe/translate at non-optimal angles, which may reduce maximum achievable speed compared to tangent heading where the robot moves "forward" naturally.

Linear Heading Interpolation

Smoothly rotates from the path's start heading to a target end heading over the course of the segment.

Usage:

// Start at 0°, gradually rotate to 90° while moving
Path path = new BezierLine(new Point(48, 0, Math.toRadians(90), Point.CARTESIAN))
    .setLinearHeadingInterpolation();

// Target heading taken from end point's heading value

Specifying Target Heading:

// Method 1: Use end point's heading
new BezierLine(new Point(48, 0, Math.toRadians(90), Point.CARTESIAN))
    .setLinearHeadingInterpolation();

// Method 2: Explicitly specify target heading
new BezierLine(new Point(48, 0, Point.CARTESIAN))
    .setLinearHeadingInterpolation(Math.toRadians(90));

Best Use Cases:

Scoring Approach: Align to backdrop/goal while driving toward it
Setup for Next Move: Pre-rotate for the next path segment
Smooth Transitions: Avoid sudden rotation jerks between segments
Predictable Rotation: Know exactly when robot will reach target heading

Example: Approach and Align

// Robot starts facing 0°, drives to backdrop while rotating to face it (90°)
follower.followPath(
    new BezierCurve(
        new Point(0, 0, Point.CARTESIAN),
        new Point(0, 0, Point.CARTESIAN),
        new Point(24, 12, Point.CARTESIAN),
        new Point(36, 36, Point.CARTESIAN),
        new Point(48, 48, Math.toRadians(90), Point.CARTESIAN)
    ).setLinearHeadingInterpolation()
);

// By the time robot reaches (48, 48), it's already facing 90°

💡 Tip: Heading Math

Linear interpolation always takes the shortest rotation path. If starting at 350° and targeting 10°, it rotates 20° forward, not 340° backward. Use Math.toRadians() for degree inputs.

Tangent Heading Interpolation

Robot always points in the direction it's currently moving along the path (tangent to the curve). Most natural-looking movement for curved paths.

Usage:

Path path = new BezierCurve(
    new Point(0, 0, Point.CARTESIAN),
    new Point(0, 0, Point.CARTESIAN),
    new Point(24, 24, Point.CARTESIAN),
    new Point(48, 24, Point.CARTESIAN),
    new Point(48, 0, Point.CARTESIAN)
).setTangentHeadingInterpolation();

// Robot heading follows curve direction automatically

Best Use Cases:

High-Speed Curves: Minimizes lateral forces, allows faster movement
Natural Movement: Looks more like human driving, less robotic
Efficient Power Usage: Robot moves "forward" relative to itself
Complex Paths: Automatically handles heading on intricate curves

Example: Smooth S-Curve

// Robot follows S-shaped path, always pointing where it's going
follower.followPath(
    new BezierCurve(
        new Point(0, 0, Point.CARTESIAN),
        new Point(0, 0, Point.CARTESIAN),
        new Point(16, 16, Point.CARTESIAN),
        new Point(32, -16, Point.CARTESIAN),
        new Point(48, 0, Point.CARTESIAN)
    ).setTangentHeadingInterpolation()
);

// Heading automatically adjusts through the S-curve

⚠️ Watch Out For

Tangent heading on very tight curves can cause rapid rotation. If the robot spins too quickly, consider using linear or constant heading instead, or make the curve gentler with adjusted control points.

Mixing Interpolation Strategies

You can use different interpolation methods for different path segments to optimize for each phase of movement.

Example: Complete Autonomous Sequence

follower.followPath(
    // 1. Fast approach with tangent heading (efficient, high speed)
    new BezierCurve(
        new Point(0, 0, Point.CARTESIAN),
        new Point(0, 0, Point.CARTESIAN),
        new Point(20, 10, Point.CARTESIAN),
        new Point(30, 20, Point.CARTESIAN),
        new Point(40, 24, Point.CARTESIAN)
    ).setTangentHeadingInterpolation(),

    // 2. Strafe to exact position while maintaining angle (constant heading)
    new BezierLine(new Point(42, 30, Point.CARTESIAN))
        .setConstantHeadingInterpolation(Math.toRadians(90)),

    // 3. Final alignment while approaching (linear heading)
    new BezierLine(new Point(44, 36, Math.toRadians(135), Point.CARTESIAN))
        .setLinearHeadingInterpolation()
);

// Each segment uses the best strategy for its purpose

Strategy Selection Guide:

TangentSpeed priority, curved paths

LinearEnd alignment, smooth rotation

ConstantMechanism orientation, strafing

Custom Heading Functions

For advanced users: create custom heading interpolation by extending the Path class.

// Example: Custom sinusoidal heading variation
public class OscillatingHeadingPath extends BezierLine {
    private double frequency;
    private double amplitude;

    public OscillatingHeadingPath(Point endPoint, double freq, double amp) {
        super(endPoint);
        this.frequency = freq;
        this.amplitude = amp;
    }

    @Override
    public double getHeadingGoal(double t) {
        // t ranges from 0 to 1 along the path
        return Math.sin(t * frequency * 2 * Math.PI) * amplitude;
    }
}

// Robot heading oscillates while moving
Path weaving = new OscillatingHeadingPath(
    new Point(48, 0, Point.CARTESIAN),
    2.0,  // 2 full oscillations
    Math.toRadians(30)  // ±30° amplitude
);

Advanced feature - most use cases are covered by the three built-in interpolation methods.

Interpolation Tips

💡 Visualize on Dashboard

FTC Dashboard shows the robot's target heading as an arrow. Watch how it changes along the path to verify your interpolation is working as expected.

💡 Heading PID Tuning Matters

Aggressive interpolation strategies (rapid heading changes) require well-tuned heading PID. If robot can't keep up with heading targets, increase heading P or reduce rotation speed.

💡 Test Interpolation Separately

When debugging path issues, test the same path with different interpolation methods. If tangent works but linear doesn't, the problem is heading-related, not path-following.