PathWeaver Trajectory Following not working as Intended

Hello everyone, I am the lead programmer on my team and we are having some trouble with using PathWeaver and WPILib’s Trajectory class for the autoNAV Challenge. We don’t have any errors in our code or anything and our robot actually somewhat drives the path we tell it to, but only somewhat and not very consistently. For example: for the barrel racing path, we always loop around the first ball as we should, but after that the robot reacts strangely, sometimes driving differently even when we have not changed the code or the path to be driven at all.

Some theories I have: Could there be a unit conversion error somewhere? (when converting from native encoder units to meters) Could gyroscope drift be causing this as the first loop of the path seems to be completed fine? Or I understand that WPILib Trajectories are time-based, could we be missing states of the trajectory somehow?

Any help would be GREATLY appreciated, as you all know the deadline for skills challenges is soon and we were really hoping to get this to work because trajectory following is something our team has never done before.

Here is the link to our code on GitHub: InfiniteRecharge/src/main/java/org/usfirst/frc/team3042/robot at master · team3042/InfiniteRecharge (

The Drivetrain Subsystem is in “subsystems ->” and our command for driving a trajectory is in “commands -> drivetrain ->”

Convert method may need to divided resolution of encoder and gear ratio?
In our case, we calc distance per pulse of encoder:
0.1524(m) * PI / 2048 (when encoder turn around will increase 2048) / 9.7(gear ratio)

For our conversion methods shown below:

    public double positionToMeters(double position) {

        return position * Math.PI * RobotMap.WHEEL_DIAMETER / 39.3700787; // Divide by 39.3700787 to convert inches to meters


    public double speedToMeters(double speed) {

        return speed / 60 * Math.PI * RobotMap.WHEEL_DIAMETER / 39.3700787; // Divide by 39.3700787 to convert inches to meters


The parameter “position” is in number of wheel rotations and the parameter “speed” is in RPM (rotations per minute). So we are converting wheel rotations into meters and RPM into meters/sec. Given this context, do our conversion methods look right?

Have you gone through the trajectory troubleshooting steps here?


I have not but I definitely will now, thank you!

After some more research, I feel as though the issue is that we did not properly characterize our robot’s drivetrain. We didn’t fully understand the process of analyzing the data using the “data analysis” tab of the characterization tool and thus may have skipped some steps that resulted in us using bad ks, kv, ka, and kp values. I will definitely look into characterization more, and my hope is that this method of autonomous trajectory following is something that our team can figure out over the summer off-season so we have it ready for next year!

1 Like

If you find anything in the docs confusing, please feel free to open a github issue suggesting improvements.

1 Like