WPILib Trajectory Guide

One of our projects for the start of Build Season was learning the WPILib Trajectory feature which acts as a path planning and following library. In this post, I will cover what we did to achieve path planning using WPILib Trajectory.

The example code is at this repo

Introduction to WPILib Trajectory

WPILib Trajectory is based the recommended way for pathfinding starting in 2020. It is based off of the 254 2018 autonomous and significantly reduces the computation time associated with path planning as well as accuracy. I am not going to go over the details of it, but FRC 4915 has a great video explaining how it works. I highly recommend you look over it, even though you don’t need to know the details, because it is fascinating.

A Deep Dive into WPILib Trajectories

This article is going to outline the steps that my team took to get from nothing to working path-following using WPILib Trajectory. Most of the code should be self-documenting, but there will be commentary throughout explaining what is going on.

Background

My team has struggled with path-following for a long time. We tried to get Jaci Pathfinder working in the past, but unreliable robots combined with a lack of control systems experience meant that it never took off. In 2019, we decided to call off attempts at autonomous code in order to prioritize state machines and vision-based navigation (which we did not have enough time to put on the production robot). We had been talking for a while that we wanted to do path-planning in 2020, and we began work in the off-season. Due to the responsibilities of the Software sub-team in preparing a robot for CalGames and Madtown ThrowDown, path planning work primarily ended up occurring in the holiday break, and took a long time to get ready. I decided on January 6 to reevaluate path-planning from scratch using WPILib Trajectory, and got a working version much faster than Jaci Pathfinder. Much of my increase in speed was due to my recent experience with Jaci Pathfinder.

For both Jaci Pathfinder and WPILib Trajectory, we did feedback using a PID loop on the motor controllers, so we did not use the voltage controls (which will be mentioned later on).

Our current plan for the production robot is to use REV Robotics Spark Max motor controllers with NEO motors, so we will have to do conversions to their native units rather than the Talon SRX ones, but other than that the idea is the same.

Step 1: Learn Your Robot

The best way to start off is to learn your robot. This makes sure that you know exactly what you are controlling. Some questions to ask yourself are:

• What motor controllers am I using?
• Do I have a master-follower setup on my drivetrain? (A follower motor [like a Victor SPX] following the actions of a master [like a Talon SRX])
• What encoders am I using?
• What is the resolution of the encoders I am using?
• What gyroscope or IMU am I using?
• What is the gearing ratio from the encoder to the wheel?
• Does it make sense for us to control on the motor controller or RoboRIO?

Step 2: Characterize Your Robot

WPILib Trajectory relies on feedback to ensure that the drivetrain drives accurately. For this reason, you need to figure out the parameters for your control loop. These parameters will be involved in calculations to ensure reliable driving, so it is important to make sure that you put the proper values in.

What Units Are Needed?

There are two main ways for feedback parameters on the robot. One is a PIDF controller, which should be used if you are using a motor controller for feedback (Talon SRX, Talon FX, Spark MAX). The other options are voltage feedback, which should be used if you are doing controls with a RoboRIO.

PIDF Controller

A PIDF controller needs four main values:

• kP: Proportional
• kI: Integral
• kD: Derivative
• kF: Feedforward

These values will all be factored into the computation. The kF feedforward value is added to the result, regardless of error. kF plays the role of keeping the robot on track, while kP, kI, and kD all play into error correction. For this reason, your robot should be able to drive fairly accurately with only kF (which will be mentioned later).

Voltage Controller

A Voltage controller needs three main values:

• kS: Position
• kV: Velocity
• kA: Acceleration

Options for Obtaining Values

Option 1: FRC Characterization

FRC Characterization is an app from WPILib that lets you run software on your robot to identify the proper values for your robot.

The information about how to characterize the robot is below:

Introduction to Robot Characterization — FIRST Robotics Competition documentation

Characterization works best if you are using feedback on the RoboRIO, rather than a motor controller like a Talon SRX, Talon FX, or Spark MAX. The values returned will be voltage control constants.

Option 2: Tune Your Own PID

The other option, which works better for motor-controller based autonomous is to tune your own PID. The way we tuned this is by writing a simple app that has kP, kI, kD, kF, and Velocity for configuration on SmartDashboard, as well as the active sensor velocity and error for plotting. This makes it super easy to find the values for your motor controller PID loop. The steps to tune are:

• Set kP, kI, kD, and kF to 0
• Set a velocity to a realistic value (I chose 3000 ticks/100ms which was slow but visible for the robot we tested on)
• Gradually increase kF until the sensor velocity is very close to the desired velocity
• Change the velocity, and re increase kF until it is close
• Change the velocity again, and add a tiny bit of kP
• Change the velocity, change kP (usually initially by a factor of 10, make smaller later) until it gets very close to the desired velocity without overshooting
• If stability remains an issue, try adding a very tiny amount of kD
• Test different velocity setpoints and see how close it can get to the setpoint
• Record your values

Having Problems?

There are some things to make sure are set properly if you are encountering issues with your characterization process:

• Are the encoders sending data?
• Are all units correct?
• Are all of the motors set to the correct inversion?
• Are all of the sensors set to the correct inversion (sensor phase)?
• Does the gyroscope need to be inverted?
• Is the gyroscope/IMU returning data?
• Are the SmartDashboard values updating to the motor controllers?
• Are all of the motor controller and feedback parameters set properly?

Step 3: Create a New Project

Now you need to make your robot project. If you don’t have a project, open VS Code for your year, press CTRL + SHIFT + P at the same and create a new project. We used a new Command Subsystem structure (starting 2020) for our project.

3a: Add Vendor Libraries

It usually is a good idea to start of by adding any vendor libraries that are needed. We added Phoenix for our project, since CTRE devices were the only non-WPILib-supported devices on our minibot test-bed. Adding vendor libraries early on will allow you to have autocomplete throughout the entire testing process.

3b: Create a Constants.java

In order to keep your code organized and configuration simple, it can be very helpful to have a file that tracks all constants. If you want to be even more organized, create subclasses in your file to separate different components. A short excerpt of our Constants.java file is below:

package frc.robot;

public class Constants {
    public class RobotCharacteristics {
        public static final double WHEELBASE_WIDTH = 0.4572; // Meters
    }

    public class Auto {
        public static final double DT = 0.05;
        public static final double MAX_VELOCITY =1;
        public static final double MAX_ACCELERATION = .25;
        public static final double MAX_JERK = .25;
    }
    ...
}

3c: Create a new Subsystem with the Required Configuration

After you have your vendor libraries, it is time to create a Subsystem with all of the configurations set. Your motor controllers and gyroscope/IMU should be instance variables that are initialized in the constructor. Your configuration for each motor will depend on what motor controllers you use, but an example for a drivetrain with two Talon SRX motor controllers is below:

public Drivetrain() {
    leftLeader = new TalonSRX(Constants.LeftLeader.CAN_ID);
    //leftFollower = new VictorSPX(Constants.//LeftFollower.CAN_ID);

    rightLeader = new TalonSRX(Constants.RightLeader.CAN_ID);
    //rightFollower = new VictorSPX(Constants.//RightFollower.CAN_ID);


    ///////////////////////////////////////////////////////////////
    
    // Reset all of the motor controllers
    // This is so that we can avoid having left over settings changes messing stuff open

    leftLeader.configFactoryDefault();
    //leftFollower.configFactoryDefault();

    rightLeader.configFactoryDefault();
    //rightFollower.configFactoryDefault();

    ///////////////////////////////////////////////////////////////

    // Have the followers follow the masters

    //leftFollower.follow(leftLeader);
    //rightFollower.follow(rightLeader);

    ///////////////////////////////////////////////////////////////

    // Configure inverts

    leftLeader.setInverted(Constants.LeftLeader.INVERTED);
    //leftFollower.setInverted(Constants.//LeftFollower.INVERTED);

    rightLeader.setInverted(Constants.RightLeader.INVERTED);
    //rightFollower.setInverted(Constants.//RightFollower.INVERTED);
    
    ///////////////////////////////////////////////////////////////

    // Configure PID

    leftLeader.config_kP(0, Constants.LeftLeader.KP);
    leftLeader.config_kI(0, Constants.LeftLeader.KI);
    leftLeader.config_kD(0, Constants.LeftLeader.KD);
    leftLeader.config_kF(0, Constants.LeftLeader.KF);

    rightLeader.config_kP(0, Constants.RightLeader.KP);
    rightLeader.config_kI(0, Constants.RightLeader.KI);
    rightLeader.config_kD(0, Constants.RightLeader.KD);
    rightLeader.config_kF(0, Constants.RightLeader.KF);

    ///////////////////////////////////////////////////////////////

    // Configure Encoder
    leftLeader.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, Constants.LeftLeader.Feedback.PORT, Constants.LeftLeader.TIMEOUT);
    rightLeader.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, Constants.RightLeader.Feedback.PORT, Constants.RightLeader.TIMEOUT);

    ///////////////////////////////////////////////////////////////

    // Configure Sensor Phase

    leftLeader.setSensorPhase(Constants.LeftLeader.Feedback.SENSOR_PHASE);
    rightLeader.setSensorPhase(Constants.RightLeader.Feedback.SENSOR_PHASE);

    ///////////////////////////////////////////////////////////////

    // Reset the Encoders

    leftLeader.setSelectedSensorPosition(0);
    rightLeader.setSelectedSensorPosition(0);

    ///////////////////////////////////////////////////////////////

    // Closed Loop Error
    leftLeader.configAllowableClosedloopError(0, Constants.LeftLeader.CLOSED_LOOP_ERROR, Constants.LeftLeader.TIMEOUT);
    rightLeader.configAllowableClosedloopError(0, Constants.RightLeader.CLOSED_LOOP_ERROR, Constants.RightLeader.TIMEOUT);

    ///////////////////////////////////////////////////////////////

    // Set Status Frame Period
    leftLeader.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, Constants.LeftLeader.STATUS_FRAME, Constants.LeftLeader.TIMEOUT);
    rightLeader.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, Constants.RightLeader.STATUS_FRAME, Constants.RightLeader.TIMEOUT);

    ///////////////////////////////////////////////////////////////

    // Current Limit

    if(Constants.LeftLeader.Power.CURRENT_LIMIT){
      leftLeader.configPeakCurrentLimit(Constants.LeftLeader.Power.MAX_AMP);
    }

    if(Constants.RightLeader.Power.CURRENT_LIMIT){
      rightLeader.configPeakCurrentLimit(Constants.RightLeader.Power.MAX_AMP);
    }

    leftLeader.setNeutralMode(NeutralMode.Brake);
    rightLeader.setNeutralMode(NeutralMode.Brake);


    //ahrs = new AHRS(SPI.Port.kMXP);
    gyro = new ADXRS450_Gyro();
    odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));

  }

As you can see, we set everything to values from Constants.java, so if we ever need to change something, we can easily find it there rather than searching through the untidy configuration code.

I won’t go over what each value does, but you can find it in CTRE documentation.

3d: Create Conversion Factors

This is where keeping track of your conversions matter. You may have seen before that there are some functions that have conversion factors as one of their parameters. By creating functions with our conversions, it is very clear what we are doing, and we can change all of our logic in one place if we do not write implement a conversion properly. Our conversion is fairly simple to see, such as below (at the time of writing, the JavaDoc comments were not done for the project, which is why some methods are missing a JavaDoc comment):

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot.frclib.AutoHelperFunctions;


/**
 * Add your docs here.
 */
public class AutonConversionFactors {
    /**
     * Convert the values from native Talon units (ticks/100ms) to WPITrajectory Units (m/s)
     * @param talonVelocity
     * @param wheelDiameter
     * @param usingMetric
     * @param ticksPerRevolution
     * @return
     */
    public static final double convertTalonSRXNativeUnitsToWPILibTrajecoryUnits(double talonVelocity, double wheelDiameter, boolean usingMetric, int ticksPerRevolution){
        double result = talonVelocity;
        result = result*10; //Convert ticks/100ms to ticks/sec
        
        double circumference = 0;
        if(usingMetric){
            circumference = Math.PI * wheelDiameter;
        }else{
            double diameterInMeters = wheelDiameter*0.3048;
            circumference = Math.PI*diameterInMeters;
        }
        double metersPerTick = circumference/ticksPerRevolution;
        result = result *metersPerTick;
        return result;
    }

    /**
     * Convert the values from WPITrajectory Units (m/s) to native Talon units (ticks/100ms)
     * @param metersPerSecond
     * @param wheelDiameter
     * @param givenMetric
     * @param ticksPerRevolution
     * @return
     */
    public static final double convertWPILibTrajectoryUnitsToTalonSRXNativeUnits(double metersPerSecond, double wheelDiameter, boolean givenMetric, int ticksPerRevolution){
        double result = metersPerSecond;
        double circumference = 0;
        if(givenMetric){
            circumference = Math.PI * wheelDiameter;
        }else{
            double diameterInMeters = wheelDiameter*0.3048;
            circumference = Math.PI*diameterInMeters;
        }
        double ticksPerMeter = ticksPerRevolution/circumference;
        result = result * ticksPerMeter;
        result = result * .1;
        
        return result;
    }


    public static double convertTalonEncoderTicksToMeters(int ticks, double diameter, double ticksPerRevolution, boolean givenMetric){
        double result = ticks;
        double circumference = 0;
        if(givenMetric){
            circumference = Math.PI *diameter;
        }
        else{
            double diameterInMeters = diameter*0.3048;
            circumference = Math.PI*diameterInMeters;
        }
        double metersPerTick = circumference/ticksPerRevolution;
        result = result * metersPerTick;
        return result;
    }

    public static double convertFeetToMeters(double value){
        return value * 0.3048;
    }
    public static double convertMetersToFeet(double value){
        return value * 1/0.3048;
    }
}

Make sure that everything is in the correct units throughout the entire process, and conversion factors for a Spark MAX/NEO will be different.

3e: Add the required methods to your Drivetrain

The drivetrain needs some methods in order to be able to drive. These functions, including our implementations, are below (note that some of the conversion factors will be added later)

public Pose2d getPose(){
    return odometry.getPoseMeters();
  }

  public DifferentialDriveWheelSpeeds getWheelSpeeds(){
    return new DifferentialDriveWheelSpeeds(AutonConversionFactors.convertTalonSRXNativeUnitsToWPILibTrajecoryUnits(this.leftLeader.getSelectedSensorVelocity(), Constants.DTConstants.WHEEL_DIAMETER, false, Constants.DTConstants.TICKS_PER_REV), AutonConversionFactors.convertTalonSRXNativeUnitsToWPILibTrajecoryUnits(this.rightLeader.getSelectedSensorVelocity(), Constants.DTConstants.WHEEL_DIAMETER, false, Constants.DTConstants.TICKS_PER_REV));
  }


  public void resetOdometry(Pose2d pose){
    resetEncoders();
    odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
  }

  public void tankDriveVolts(double leftVolts, double rightVolts){
    System.out.println(leftVolts + ","+ rightVolts);  
    leftLeader.set(ControlMode.Current, leftVolts);
    rightLeader.set(ControlMode.Current, -rightVolts);
  }

  public void tankDriveVelocity(double leftVel, double rightVel){
    System.out.println(leftVel + ","+ rightVel);  

    double leftLeaderNativeVelocity = AutonConversionFactors.convertWPILibTrajectoryUnitsToTalonSRXNativeUnits(leftVel, Constants.DTConstants.WHEEL_DIAMETER, false, Constants.DTConstants.TICKS_PER_REV);
    double rightLeaderNativeVelocity = AutonConversionFactors.convertWPILibTrajectoryUnitsToTalonSRXNativeUnits(rightVel, Constants.DTConstants.WHEEL_DIAMETER, false, Constants.DTConstants.TICKS_PER_REV);

    this.leftLeader.set(ControlMode.Velocity, leftLeaderNativeVelocity);
    this.rightLeader.set(ControlMode.Velocity, rightLeaderNativeVelocity);

    SmartDashboard.putNumber("LeftIntentedVelocity", leftLeaderNativeVelocity);
    SmartDashboard.putNumber("LeftIntendedVsActual", leftLeaderNativeVelocity-this.leftLeader.getSelectedSensorVelocity());
  }

  public void resetEncoders(){
    leftLeader.setSelectedSensorPosition(0);
    rightLeader.setSelectedSensorPosition(0);
  }

  public double getAverageEncoderDistance(){
    return (leftLeader.getSelectedSensorPosition() + rightLeader.getSelectedSensorPosition())/2.0;
  }
  public void zeroHeading() {
    gyro.reset();
  }

  public double getHeading(){
    //return Math.IEEEremainder(gyro.getAngle(), 360);
    return -1 * Math.IEEEremainder(gyro.getAngle(),360);

  }
  public double getTurnRate(){
    return gyro.getRate();
  }
}

It can be helpful to understand some of the components of this code:

• A Pose2d is an object that contains information about where the robot is, which the RamseteController will use for calculations.
• getWheelSpeeds() returns the current velocity of the drivetrain in WPILib units
• resetOdometry() will reset all of the information about the robot being used in the Trajectory calculations from a given Pose2d
• tankDriveVolts() will drive a tank drive (like a West Coast Drive) using volts (for RoboRIO Feedback) and tankDriveVelocity() will drive the robot using velocity closed-loop
• resetEncoders() will reset the value of all of the encoders so that they don’t affect running different paths
• getAverageEncoderDistance() takes the mean of the encoder distances, which can be helpful for running the unicycle controller (which bases calculations based off of the center line)
• zeroHeading() will reset the gyroscope or IMU, so that Trajectory can have a clean slate for calculating angle
• getHeading() will return the heading on a 0-360 scale (due to Math.IEEEremainder()) in degrees
• getTurnRate() returns the active rate at which the robot is turning, which is used for accounting for error in Trajectory.

Step 4: Extend the RamseteController

TIP: Having debugging in the RamseteController can be a live-saver down the line, because you can see the errors (eX, eY, eTheta) that the calculator sees. Having your own class that extends will make your life a million times easier.

And the best thing is that I have already written one that you can use, based on the code from WPILib.

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot.frclib.AutoHelperFunctions;

import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.Trajectory.State;
import frc.robot.Constants;

/**
 * Add your docs here.
 */
public class CustomRamseteControllerAbstraction extends RamseteController{
    
    private Pose2d m_poseError;
    private Pose2d m_poseTolerance;
    private final double m_b;
    private final double m_zeta;

    
    public CustomRamseteController(double b, double zeta){
        super(b, zeta);
        m_b = b;
        m_zeta = zeta;
    }
    public CustomRamseteControllerAbstraction(){
        super(2.0, 0.7);
        this.m_b = 2.0;
        this.m_zeta = 0.7;
    }
    
    private static double sinc(double x) {
        if (Math.abs(x) < 1e-9) {
          return 1.0 - 1.0 / 6.0 * x * x;
        } else {
          return Math.sin(x) / x;
        }
      }
    

    @Override
    public ChassisSpeeds calculate(Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters,
            double angularVelocityRefRadiansPerSecond) {
                this.m_poseError = poseRef.relativeTo(currentPose);

                // Aliases for equation readability
                final double eX = m_poseError.getTranslation().getX();
                final double eY = m_poseError.getTranslation().getY();
                final double eTheta = m_poseError.getRotation().getRadians();
                final double vRef = linearVelocityRefMeters;
                final double omegaRef = angularVelocityRefRadiansPerSecond;

                SmartDashboard.putNumber("Current X", currentPose.getTranslation().getX());
                SmartDashboard.putNumber("Reference X", poseRef.getTranslation().getX());

                SmartDashboard.putNumber("eX", eX);
                SmartDashboard.putNumber("eY", eY);
                SmartDashboard.putNumber("eTheta", eTheta);
                SmartDashboard.putNumber("vRef", vRef);
                SmartDashboard.putNumber("omegaRef", omegaRef);
                

            
                double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));

                SmartDashboard.putNumber("k",k);

                SmartDashboard.putNumber("vX [m/s]",vRef * m_poseError.getRotation().getCos() + k * eX);
                SmartDashboard.putNumber("vY [m/s]", 0.0);
                SmartDashboard.putNumber("vOmega [rad/s]", omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY);

                SmartDashboard.putNumber("vX [t/100ms]", AutonConversionFactors.convertWPILibTrajectoryUnitsToTalonSRXNativeUnits(vRef * m_poseError.getRotation().getCos() + k * eX, Constants.DTConstants.WHEEL_DIAMETER, false, Constants.DTConstants.TICKS_PER_REV));
            
                return new ChassisSpeeds(vRef * m_poseError.getRotation().getCos() + k * eX,
                                         0.0,
                                         omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY);
        
    }
    public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
        return calculate(currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond,
            desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
      }
    
}

Now you can drop in the CustomRamseteController anywhere that a RamseteController is allowed and have updating to the SmartDashboard. Step 5: Write a Function to Generate Trajectories

Having a function that can generate trajectories means that you can feed any set of points into your path planner and have it work. All you need is a function that takes in parameters and returns a Command. The version below is a slightly modified version from the WPILib Docs:

public Command createAutoNavigationCommand(Pose2d start, List<Translation2d> waypoints, Pose2d end) {
    System.out.println("Creating Auto Command");
    var autoVoltageConstraint = new DifferentialDriveVoltageConstraint(
        new SimpleMotorFeedforward(Constants.DTConstants.KS, Constants.DTConstants.KV, Constants.DTConstants.KA),
        Constants.DTConstants.kDriveKinematics, 10);

    // Create config for trajectory
    TrajectoryConfig config = new TrajectoryConfig(Constants.Auto.MAX_VELOCITY, Constants.Auto.MAX_ACCELERATION)
        // Add kinematics to ensure max speed is actually obeyed
        .setKinematics(Constants.DTConstants.kDriveKinematics)
        // Apply the voltage constraint
        .addConstraint(autoVoltageConstraint);

    // An example trajectory to follow. All units in meters.
    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(start, waypoints, end, config);
    System.out.println("Generated Trajectory");
    RamseteCommand ramseteCommand = new RamseteCommand(trajectory, m_drivetrain::getPose,
        new CustomRamseteControllerAbstraction(Constants.DTConstants.RAMSETE_B, Constants.DTConstants.RAMSETE_ZETA),

        Constants.DTConstants.kDriveKinematics,
       
        m_drivetrain::tankDriveVelocity, m_drivetrain);

    // Run path following command, then stop at the end.
    System.out.println("Finished Creating Auto Command");
    return ramseteCommand.andThen(() -> m_drivetrain.tankDriveVolts(0, 0));
  }
}

This code block takes an initial waypoint (Pose2d start), intermediate waypoints(List<Translation2d> waypoints), and an end way-point (Pose2d end).

Step 6: Test

Everything up to now, besides PID tuning, has been off-robot and very theoretical. The last step is to test, and see if everything works. Put sample points into your generation function, and see if it drives. I would recommend starting with a straight line test, then adding curves later. Remember: you can set your waypoints in RobotContainer.java like so:

public Command getAutonomousCommand() {
    // An ExampleCommand will run in autonomous
    Pose2d start = new Pose2d(0,0,new Rotation2d(0));
    List<Translation2d> waypoints = List.of(
      new Translation2d(1,.5),
      new Translation2d(5,1)

    );
    Pose2d end = new Pose2d(7, 0, Rotation2d.fromDegrees(-90));
    return this.createAutoNavigationCommand(start, waypoints, end);

  }

Another good tool, especially for the straight line, is using blue painter’s tape. On my first linear test, I told the robot to go nine feet forward, measured exactly nine feet with a tape measure, and put blue tape. Be careful to remember which side of the line you should be measuring from. In this test (which was my first with the auto and used a straight line), the back of the robot should’ve lined up with the blue tape, and it got very close.

The tape allowed me to see that there was a slight turn to the left while driving, which a tiny bit more tuning was able to fix.

Now you have an autonomous path! If you have problems, I would recommend looking at the WPILib Trajectory Tutorial, WPILib JavaDoc, or Chief Delphi.