Pedro Pathing 2.0.0 has released! If you haven't already, migrate now.
Pedro Pathing

Autonomous pathing,

Designed to revolutionize autonomous pathing in FTC, Pedro Pathing uses a custom path following algorithm designed for FTC to follow Bézier curves while maintaining speed, consistency, and efficiently correcting for external disturbances.

Easy to setup, easy to tune, and built for FTC.

path = follower.pathBuilder()
  .addPath(
    new BezierLine(scorePose, pickupPose)
  )
  .setLinearHeadingInterpolation(
    scorePose.getHeading(),
    pickupPose.getHeading()
  )
  .addPath(
    new BezierLine(pickupPose, scorePose)
  )
  .setLinearHeadingInterpolation(
    pickupPose.getHeading(),
    scorePose.getHeading()
  )
  .build();

follower.followPath(path);