Summary

To utilize the Pure Pursuit functionality, your drivetrain class should extend DriveTrainBase and implement the methods it specifies properly. Units should be inches and seconds. For Talons, keep in mind that the default unit for the Talon API is sensor units per 100 milliseconds, so implement your velocity methods accordingly. As for your rotation angle, we typically use 90 degrees as the starting angle. With a PigeonIMU, we simply add 90 degrees to the angle given by the API. You might have to negate the angle before adding 90 degrees, depending on which gyro you use. The idea is that on an xy-plane from the perspective of a driver, moving forward increases the y-coordinate, and moving to the right increases the x-coordinate.

Before anything else relating to Pure Pursuit is initialized, your drivetrain must be given to warriorlib. Also, the PoseEstimator must be used in order to track the robot's position. Here is a basic example demoing the path generator and tracker as well.

DriveTrainBase driveTrain;
PoseEstimator poseEstimator;
Looper autoLooper;
PurePursuitTracker purePursuitTracker;

@Override
public void robotInit() {
    driveTrain = YourDriveTrainClass.getInstance(); //singleton pattern is recommended
    DriveTrainBase.setDriveTrain(driveTrain);

    autoLooper = new Looper(1 / 50D); //recommended loop time
    poseEstimator = PoseEstimator.getInstance();
    autoLooper.addLoops(poseEstimator);

    PathGenerator pathGenerator = new PathGenerator(Constants.spacing);
    pathGenerator.addPoint(new Vector(0, 0));
    pathGenerator.addPoint(new Vector(0, 30));
    pathGenerator.addPoint(new Vector(70, 60));
    pathGenerator.addPoint(new Vector(70, 80));
    pathGenerator.addPoint(new Vector(70, 103));
    pathGenerator.setSmoothingParameters(Constants.a, Constants.b, Constants.tolerance);
    pathGenerator.setVelocities(Constants.maxVel, Constants.maxAccel, Constants.maxVelk);
    Path path = pathGenerator.generatePath();

    purePursuitTracker = PurePursuitTracker.getInstance();
    purePursuitTracker.setRobotTrack(Constants.robotTrack);
    purePursuitTracker.setFeedbackMultiplier(Constants.kP);
    purePursuitTracker.setPath(path, Constants.lookaheadDistance);
}

@Override
public void autonomousInit() {
    driveTrain.resetEncoders();
    driveTrain.resetGyro();
    poseEstimator.reset();
    purePursuitTracker.reset();
    autoLooper.start();

    AutoModeExecuter autoModeExecuter = new AutoModeExecuter();
    autoModeExecuter.setAutoMode(new PurePursuitTestMode());
    autoModeExecuter.start();
}

Note that it is recommended to stop the auto looper in disabledInit, teleopInit, etc., as well as reset the drivetrain, pose estimator, and Pure Pursuit tracker whenever possible. In this case PurePursuitTestMode is a simple auto mode that runs the PurePursuitAction.

public class PurePursuitTestMode extends AutoModeBase {
    @Override
    protected void routine() throws AutoModeEndedException {
        runAction(new PurePursuitAction());
    }
}

Details

• For path smoothing, a = 1-b and both a and b should be between 0 and 1. Ideally b should fall in the 0.7-0.9 range. The higher the b value, the smoother the path.
• spacing is the distance between points along the path: the path generator will inject points along the coordinates you specify, at the given spacing.
• tolerance is the maximum change per single smoothing iteration. If tolerance is exceeded, the smoothing algorithm runs again. This repeats until the change in the path is below the tolerance, which should be set around 0.001. If this value is too low, the smoothing might never converge, so try raising the tolerance if this occurs.
• lookaheadDistance is simply the distance of how far along the path we should "pursue", typically 15-24 inches
• maxVel and maxAccel are in in/s and in/s^2 respectively. For reference our 2019 robot uses 276 in/s for max velocity and 100 in/s^2 for max acceleration.
• maxVelk is proportional to how fast we turn, and should be set between 1-5
• feedback is supported, where kP should be around 0.001. The higher kP is, the more we compensate for being off the targeted velocity. However, too high of a kP value will cause oscillations.
• warriorlib is designed to process Pure Pursuit velocities resulting in inches per second, rather than a scaled value from -1 to 1. Make sure you implement your drivetrain methods accordingly. Using other units (for time especially) may cause unexpected behavior.
• PurePursuitAction resets the drivetrain, as well as the pose estimator and tracker upon starting and finishing. While running, it updates the tracker and sets drivetrain velocities accordingly. Make sure all methods are implemented properly in order to ensure intended behavior.
• generating paths in robot code is not recommended, because of time constraints and the increased complexity of multiple/longer paths. We are working on implementing saving and reading paths from files.

References

• We used Team 1712's Pure Pursuit whitepaper for this implementation. To truly understand the ins and outs of the algorithm, read this.
• Code for advanced pose estimation was based on Team 254’s implementation.