Skip to content
MARSLib Documentation

Pathfinding & Autonomous

To win on Einstein, a robot cannot simply drive straight lines. The autonomous period is an exercise in Discovery and Innovation. The field is littered with obstacles, and sudden game piece collisions require dynamic recalculations. MARSLib integrates deeply with PathPlanner to bring seamless, holistic pathfinding to the FRC field—turning complex trajectory math into a Fun and rewarding experience for every programmer.

1 Catmull-Rom Splines

Section titled “1 Catmull-Rom Splines”

Instead of hardcoding angles and trapezoidal movement profiles manually, we use Catmull-Rom splines. Splining allows the robot’s Holonomic Drive Controller to continuously traverse a curved trajectory without having to decelerate at individual waypoints.

Try dragging the waypoints (nodes) on the field simulator below to watch how the trajectory natively curves to intercept them smoothly. Press FOLLOW SPLINE to witness the holonomic controller drive the trajectory:

2 A* NavGrid Traversal

Section titled “2 A* NavGrid Traversal”

During the tele-op period, the driver may need to command the robot to auto-align or traverse from the Loading Zone directly to the Hub. MARSLib uses a NavGrid A* solver running on the Driver Station (or Coprocessor) to intelligently weave a safe path around the Stage/Ladder obstacles.

// Dynamically request a path to a scoring node safely avoiding the Stage
Command autoScoreCmd = AutoBuilder.pathfindToPose(
    new Pose2d(14.5, 5.5, Rotation2d.fromDegrees(180)),
    new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720)),
    0.0,
    0.0
);

3 Fusing Vision with Odometry

Section titled “3 Fusing Vision with Odometry”

Path execution relies 100% on where the robot thinks it is on the field. Without AprilTags, the wheel encoders slowly drift over time. This implies that your Autonomous Splines will gradually shift away from their hardcoded positions as the match progresses.

Path execution relies 100% on where the robot thinks it is on the field. Without AprilTags, the wheel encoders slowly drift over time. This implies that your Autonomous Splines will gradually shift away from their hardcoded positions as the match progresses.

To guarantee mm-perfect precision, we feed the Limelight 4’s MegaTag 2.0 poses directly into the SwerveDrivePoseEstimator. This organically corrects the internal map as the robot drives, ensuring PathPlanner always knows the true distance remaining to the next Spline knot!

4 Autonomous Execution Reference

Section titled “4 Autonomous Execution Reference”

AutoBuilder Initialization

Section titled “AutoBuilder Initialization”
public class RobotContainer {
    public RobotContainer() {
        // Configure PathPlanner AutoBuilder
        AutoBuilder.configure(
            this::getPose,                    // Pose supplier
            this::setPose,                    // Pose consumer
            this::getRobotRelativeSpeeds,     // ChassisSpeeds supplier
            this::driveRobotRelative,         // ChassisSpeeds consumer
            new PathConstraints(3.0, 4.0, 5.0), // Max vel, accel, angular vel
            this::shouldFlipPath,             // Mirror for alliance
            this                              // Reference to subsystem
        );
    }


    // ... Subsystem getter/setters
}

Event Markers

Section titled “Event Markers”

In PathPlanner GUI, add event markers along your path named: "shoot", "intake", "aim", etc. Register them in RobotContainer BEFORE building autos:

// Register commands that PathPlanner can trigger mid-path
NamedCommands.registerCommand("shoot", new ShootCommand(shooter));
NamedCommands.registerCommand("intake", new IntakeCommand(intake));
NamedCommands.registerCommand("aim", new AimCommand(drivetrain, vision));

Path Following Tuning

Section titled “Path Following Tuning”

If the holonomic drive controller starts drifting from your spline path, manually tune the P/I/D logic:

// Create custom path follower config
PathFollowerConfig config = new PathFollowerConfig(
    5.0,    // Translational PID P (increase if robot drifts from path)
    0.0,    // Translational PID I
    0.0,    // Translational PID D
    5.0,    // Rotational PID P (increase if robot doesn't face right direction)
    0.0,    // Rotational PID I
    0.0,    // Rotational PID D
    0.5     // Max velocity error (tolerance)
);


AutoBuilder.configure(..., config, ...);

Simulation Testing

Section titled “Simulation Testing”

Test autonomous piece-by-piece via the MARSLib Dyn4j engine:

@Test
public void testAutonomous() {
    MARSTestHarness.reset();


    // Run auto
    Command auto = new PathPlannerAuto("TestAuto");
    auto.schedule();


    // Spin simulator 150 loops = 3 seconds
    for (int i = 0; i < 150; i++) {
        DriverStationSim.notifyNewData();
        SimHooks.stepTiming(0.02);
        CommandScheduler.getInstance().run();
    }


    // Assert collision-free translation logic
    Pose2d finalPose = drivetrain.getPose();
    assertTrue(finalPose.getTranslation().getDistance(new Translation2d(2.0, 1.0)) < 0.1);
}

📖 Further Reading & External Resources

Section titled “📖 Further Reading & External Resources”