Unit 9 · Lesson 8

PathPlanner: Integration and Constraint Tuning

Drawing the path is the easy part. Getting the robot to follow it accurately at competition speed requires the right telemetry, a systematic tuning workflow, and the ability to read what the robot's behavior is telling you about your gains.

By the end of this lesson, you will:

Publish the required telemetry signals to SmartDashboard and verify the AutoBuilder integration is working before running the first path
Follow the six-step tuning workflow: start with feedforward only, then add translation PID, then rotation PID, then increase constraints
Identify the five most common path-following symptoms (overshoot, lag, oscillation, heading drift, speed mismatch) and know which gain to adjust for each
Use AdvantageScope's 2D field view to compare planned vs. actual robot trajectory after a path run
Set appropriate velocity and acceleration constraints for your specific drivetrain hardware
Determine when a path error is caused by controller gains vs. odometry drift vs. path geometry

The Tuning Mindset: Observe Before You Adjust

The most common tuning mistake is changing multiple gains at once and then not knowing which change caused which improvement. Path following has four independently tunable parameters — translation kP, rotation kP, maxVelocity, and maxAcceleration — and they interact. Changing two at once makes it impossible to know which one was responsible for the change in behavior.

The rule is: change one thing, run the path, observe the result, record what changed. Every adjustment should be a hypothesis: "I believe increasing translation kP will reduce the positional lag I'm seeing in the curves." After running the path, the data confirms or refutes that hypothesis. This isn't slow — with good telemetry, you can run a path, read the result, and make the next change in under 90 seconds. Systematic tuning is faster than guessing because it converges instead of wandering.

Step Zero: Instrument Before You Run

You cannot tune what you cannot see. Before running any PathPlanner path on a real robot, publish at minimum the planned trajectory and actual robot pose to SmartDashboard or AdvantageScope. Without this, all you can observe is gross behavior (did the robot roughly go the right direction?) — not the detailed error information needed for systematic tuning.

Minimum required telemetry for path-following tuning add to DriveSubsystem.periodic()

Drive/ActualPose

Pose2d → Field2d widget The robot's current odometry pose. Send to a Field2d widget to see the robot's actual position on a field diagram. The gap between this and the planned trajectory is your tracking error — this is what you're trying to minimize.

Drive/TargetPose

Pose2d → Field2d widget overlay The trajectory's desired pose at the current timestep, retrieved from PathPlanner's active path command. Shows where the controller thinks the robot should be right now. If this tracks perfectly and the robot lags, your feedforward is too low.

Drive/TrajectoryPose

Pose2d[] → Field2d trajectory overlay The full planned trajectory as a sequence of poses, drawn on the field. Add this as a separate robot overlay in AdvantageScope. Shows the intended path shape and lets you see how far the actual robot deviates from it throughout the run.

Drive/TranslationError

double (meters) The Euclidean distance between the actual pose and the trajectory's desired pose: Math.sqrt(dx² + dy²). Log this as a number and watch it in a SmartDashboard plot during a run. A well-tuned path will keep this below 0.05 m on straight segments and below 0.15 m on tight curves.

Drive/HeadingError

double (degrees) The difference between the actual robot heading and the trajectory's desired heading. Separate from translation error — the robot can be on position but rotated incorrectly, or vice versa. Both need to be zero for a well-tracked path.

Drive/ChassisSpeeds

double[3] (vx, vy, omega) The actual measured chassis speeds from odometry. Compare against the trajectory's desired speeds to see how well the feedforward is predicting required velocity. Large gaps between desired and actual speed indicate feedforward needs improvement.

DriveSubsystem.java — telemetry for path following

private final Field2d m_field = new Field2d();

@Override
public void periodic() {
    // Update odometry every loop
    m_poseEstimator.update(m_gyro.getRotation2d(), getModulePositions());

    // Publish robot pose to Field2d widget
    m_field.setRobotPose(getPose());
    SmartDashboard.putData("Field", m_field);

    // Publish individual values for error monitoring
    SmartDashboard.putNumber("Drive/X", getPose().getX());
    SmartDashboard.putNumber("Drive/Y", getPose().getY());
    SmartDashboard.putNumber("Drive/HeadingDeg", getPose().getRotation().getDegrees());
}

// Call this from the path-following command when the trajectory sample is available:
public void logTargetPose(Pose2d target) {
    m_field.getObject("target").setPose(target);
    SmartDashboard.putNumber("Drive/TranslationError",
        getPose().getTranslation().getDistance(target.getTranslation()));
    SmartDashboard.putNumber("Drive/HeadingErrorDeg",
        Math.abs(getPose().getRotation().getDegrees()
             - target.getRotation().getDegrees()));
}

💡 AdvantageScope's 2D field view is the best tuning tool available

If you're using AdvantageKit logging (covered in Unit 11), AdvantageScope can replay a path run and show both the planned trajectory and actual robot path simultaneously as overlapping traces on a field diagram. The gap between them — visible as a spatial separation between two robot icons — is your tracking error rendered in field space. This is far more informative than a single distance number. Even without AdvantageKit, the Field2d widget in SmartDashboard shows both traces live during a run if you call m_field.getObject("trajectory").setPoses(trajectoryPoses) with the full trajectory at path start.

Gain Tuning Simulator

Adjust the gains below and run the path to see how each parameter affects tracking accuracy. Translation kP corrects position error, rotation kP corrects heading error, maxVelocity caps path speed, and maxAcceleration controls how aggressively the robot ramps. The ideal tuning produces a robot that stays close to the planned path throughout — the actual trace (purple) should nearly overlap the planned trajectory (dashed gray).

Path following gain tuner — simulated swerve drive stopped

Translation PID

kP 5.0

kD 0.00

Rotation PID

kP 3.0

kD 0.00

Path Constraints

maxV (m/s) 3.5

maxA (m/s²) 3.0

planned trajectory

actual robot path

heading arrows

Max transl. error—

Max heading error—

Duration—

Assessment—

The Six-Step Tuning Workflow

This sequence applies to every new robot and every new path season. Don't skip steps — each one validates a prerequisite for the next.

Verify odometry accuracy before any path test

Drive the robot 1 meter forward at 30% speed with a timed command. Check SmartDashboard: the pose should read approximately (1.0, 0.0). If it doesn't, fix encoder scaling, wheel diameter, or gear ratio before continuing. Path following cannot compensate for systematic odometry error — it amplifies it. Don't proceed until odometry is accurate within 3 cm over 1 meter.

Set translation and rotation kP to zero — test feedforward only

Configure PPHolonomicDriveController with PIDConstants(0, 0, 0) for both translation and rotation. Run a simple straight path at conservative constraints (maxVelocity = 2.0, maxAcceleration = 2.0). Observe how well the robot tracks the path using velocity feedforward alone. A well-characterized drivetrain (kV from SysId) should follow a straight path to within 10–20 cm with no PID correction. If it's off by more than 30 cm, your drivetrain characterization or RobotConfig mass/MOI values need adjustment — not your PID gains.

Add translation kP — start at 1.0, increase until oscillation

Increase translation kP until the robot tracks the path more tightly. Watch TranslationError in SmartDashboard during a run. Good values for most FRC swerve drivetrains are between 3.0 and 8.0. Stop increasing when you see the robot's actual path oscillating around the planned path (visible as a zigzag on the Field2d overlay). Back off by 20% from the value that causes first oscillation.

Add rotation kP — start at 1.0, increase until heading oscillation

With translation kP set, increase rotation kP until the robot's heading tracks the planned heading closely. Watch HeadingError in SmartDashboard. Good values are typically 2.0–5.0. Rotation oscillation is visible as the robot rocking back and forth around a heading setpoint. Back off by 20% from first oscillation.

Increase maxVelocity in 0.5 m/s increments

With gains tuned, increase maxVelocity while watching for two failure modes: motors tripping current limits (visible as velocity drop-outs in the speed plot) or translation error increasing significantly in curves. Your maximum usable velocity in curves is limited by the robot's ability to generate centripetal force without wheel slip. If you see path errors increase in curves but not straights, you've exceeded that limit.

Increase maxAcceleration in 1.0 m/s² increments

Higher acceleration means shorter ramp-up distances but more wheel slip risk. Test by running a short path (3–4 meters) at your target maxVelocity and high acceleration. Watch for the robot's actual speed lagging behind the trajectory's planned speed at the start — this indicates the motors can't supply the required acceleration. Your characterization's kA value should handle this, but physical limits (motor current) can still cap it.

🔍 Starting values that work for most competition swerve drivetrains

Based on common competition drivetrain configurations (Kraken X60 or Falcon 500, MK4i or similar, 5–7 kg robot excluding bumpers): start with translation kP = 5.0, rotation kP = 3.0, maxVelocity = 3.5 m/s, maxAcceleration = 3.0 m/s². These aren't universal — your specific robot mass, wheel grip, and characterization accuracy will shift them — but they're in the right neighborhood for an initial test rather than starting from zero. If the path looks roughly right from the first run, you're in the ballpark and can tune from there.

Reading the Robot: Five Common Symptoms

Path-following errors have recognizable visual signatures. If you can describe what you're seeing, you can identify what to change.

Robot overshoots curves

What it looks like Actual path swings wide outside the planned trajectory on curves before returning. The robot passes the outside of the curve, then corrects back inward.

The robot has too much momentum entering the curve for the controller to redirect it. Either the robot is moving too fast relative to the curve radius, or translation kP is too low to generate enough centripetal correction.

Fix: reduce maxVelocity by 0.5 m/s OR increase translation kP by 1.0

Robot consistently lags behind trajectory

What it looks like The trajectory's desired pose is always ahead of the actual robot pose throughout the path. The robot always appears "behind" the planned path, especially in straight sections.

The feedforward isn't supplying enough velocity, so the robot runs slower than planned. Either kV from characterization is too low, or the RobotConfig mass is significantly underestimated.

Fix: verify SysId kV values in RobotConfig, OR increase translation kP slightly

Robot oscillates along path

What it looks like Actual path forms a zigzag around the planned trajectory. The robot crosses the planned path repeatedly instead of tracking it. More visible at higher speeds.

Translation kP is too high. The correction overshoots the path, then overcorrects again in the other direction. Adding a small kD can dampen this without reducing kP.

Fix: reduce translation kP by 20% OR add translation kD (start at 0.1)

Robot heading drifts or spins

What it looks like Robot position tracks the path correctly but the robot is rotated incorrectly. On a holonomic path with heading constraints, the robot doesn't face the right direction at waypoints.

Rotation kP is too low to track heading changes along the path, or rotation kP is too high causing heading oscillation (rocking). Tune rotation completely independently of translation.

Fix: tune rotation kP independently — increase until oscillation, then back off 20%

Consistent endpoint position error

What it looks like Path runs well throughout, but the robot arrives at the endpoint 10–20 cm from the planned final position. The error is consistent across runs — always the same direction and magnitude.

Odometry accumulated error throughout the path. Either encoder scaling is slightly off (review Lesson 6's setDistancePerPulse calculation) or the starting pose wasn't reset to exactly match the trajectory's first waypoint.

Fix: verify odometry accuracy (Lesson 6 check) and confirm pose reset is exact

Selecting the Right Constraints for Your Robot

Velocity and acceleration constraints are not tuning parameters in the PID sense — they're physical limits. Setting them above what your hardware can execute produces trajectory plans the robot physically cannot follow, no matter how well the PID gains are tuned.

Constraints rationale — how to derive starting values for your robot

// maxVelocity: start at 70% of your measured free-speed maximum
// Free speed: run the robot at 100% for 3 seconds on carpet, measure distance.
// Free speed = distance / time (approximately).
// Starting value: free_speed_mps * 0.70
// Increase until you see tracking errors in curves, or motors trip current limits.
//
// Real example: Kraken X60, 6.75:1 L2 reduction, 4" wheel
// Free speed ≈ 5.4 m/s
// Starting maxVelocity = 5.4 * 0.70 = 3.78 → round to 3.5 m/s

// maxAcceleration: start at approximately 1.5× maxVelocity (numerical value)
// At maxVelocity = 3.5, start with maxAcceleration = 5.0 m/s²
// Reduce if:
//   - Wheels visibly slip at path start (acceleration phase)
//   - ChassisSpeeds shows robot can't keep up with ramp profile
//   - PDH shows current spikes exceeding per-module breaker rating

// maxAngularVelocity / maxAngularAcceleration (rotation constraints):
// These control how fast the robot can spin to meet heading targets.
// Start with maxAngularVelocity = 2π rad/s (one full rotation/second)
// Increase if heading changes along the path are taking too long.
// Decrease if you see the robot swaying side-to-side trying to turn while driving.

new PathConstraints(
    3.5,   // maxVelocity m/s
    5.0,   // maxAcceleration m/s²
    2 * Math.PI,  // maxAngularVelocity rad/s
    4 * Math.PI   // maxAngularAcceleration rad/s²
);

🔍 The relationship between speed, curve radius, and wheel grip

There's a hard physics limit to how fast a robot can follow a curve: centripetal acceleration = v² / r, where v is speed and r is curve radius. For a 55 kg robot moving at 4 m/s around a 1-meter radius curve, centripetal acceleration = 16 m/s². At roughly 9.8 m/s² per unit of gravity, that's 1.6g — which exceeds the lateral grip of FRC wheels on carpet (typically 0.6–0.9g). The wheels slide outward. No amount of PID tuning can overcome a physics limit. If your robot consistently overshoots tight curves at high speed, the fix is either to reduce maxVelocity or to add a constraint zone that slows the robot before the curve. This is the same principle that limits F1 cars on chicanes — grip, not code.

🔌 System Check

⚙️ Before Competition — Path Following Verification Checklist

Run this checklist before the first practice match at any event, not just during initial development:

TranslationError stays below 0.10 m throughout the path. Run your competition auto and log TranslationError. If it exceeds 0.10 m consistently in curves or 0.05 m in straights, the gains need adjustment or the path geometry needs to be simplified.
HeadingError stays below 5° throughout the path. Log HeadingError separately. A robot that tracks position accurately but has wrong heading is pointing its mechanism the wrong direction at event markers.
Pose reset is verified on the field surface. Place the robot at the starting position, enable autonomous, and log the pre-path pose. It should match the trajectory's starting waypoint to within 2 cm. If it doesn't, adjust the starting pose constant or the physical robot placement procedure.
Path runs cleanly at competition battery voltage. Battery state affects motor output, which affects feedforward accuracy. Always run a full path verification with a freshly charged competition battery, not a partially depleted test battery. The same path that tracks at 12.6V may lag noticeably at 11.8V if feedforward is tight.
Final endpoint accuracy is within your game's scoring tolerance. The path isn't "done" when it looks good in the Field2d widget. Measure the actual robot landing position against the required scoring position. If the game requires ±5 cm accuracy and your endpoint error is 8 cm, the path needs further tuning or a pose correction approach (covered in Lesson 11).

💪 Practice Prompt

Tune Your First PathPlanner Path to Competition Standard

Add the telemetry code from this lesson to your drivetrain subsystem's periodic(): Field2d with robot pose and target pose overlays, TranslationError as a number, and HeadingError in degrees. Open SmartDashboard and confirm all values update in real time. Drag the Field widget to a prominent position — you'll watch it during every path test.
Follow Step 2 of the tuning workflow: set both kP values to 0, run your test path at maxVelocity = 2.0, maxAcceleration = 2.0. Record the maximum TranslationError. If it's above 30 cm, verify your RobotConfig mass and kV values before proceeding. If it's below 20 cm, your feedforward characterization is solid.
Add translation kP = 1.0. Increase by 1.0 each run until you see oscillation in the Field2d display (zigzag pattern). Back off by 20% and record the value. Repeat for rotation kP starting at 1.0. Record both final values with comments explaining the reasoning for each.
With gains set, increase maxVelocity from 2.0 to your target in 0.5 m/s steps. At each step, run the path and log TranslationError. Stop when error increases significantly in curves. Record the maximum usable velocity and the error level at that constraint.
Bonus: Create a SmartDashboard "tuning summary" that displays all four tuned values (tkP, rkP, maxV, maxA) alongside the measured TranslationError and HeadingError from the most recent run. Use SmartDashboard.putNumber() for each. After competition, compare your final tuned values against the starting estimates in this lesson — note which values converged near the suggestion and which diverged, and hypothesize why.