Jaci's Pathfinder not responding as expected

We are trying to implement Jaci’s Pathfinding code in our java project this year, and for some reason it is outputting values significantly higher that 1 (anywhere between 5 and 5000) and it is not stopping at the appropriate distance (8 feet converted to meters equals about 11 feet)

Any ideas?

Is that the new math I am hearing so much about?

Question 1: Is your encoder ticks to distance conversion correct? If you rotate the wheel 1 rotation, are you measuring a distanced traveled of PI * WHEEL_DIAMETER?

Question 2: How are you following the generated trajectory? Do you have your own control loop or are you using the one built into Pathfinder?

In general, some snippets of the relevant code would be useful to help debugging.

We just got path finder working, heres a couple things we found along the way:

  1. It. Takes. Time. We spent atleast 30 hours tuning the code to make it work, granted, we use victors so we had to do a little bit of tweaking since its really made to use with Talons.

  2. Make sure all your units are the same, we chose feet.

  3. Scale your output, we get anywhere from 0 to 10 as L and R outputs, we just scale them in relation to eachother, for example, if L is 4 and R is 2, scale it to L=1 R=0.5

  4. Dont allow negetive motor power if you arent expecting it, we were getting negative motor powers on right turns on our L which shouldnt be correct, so we made a statement that says if its a right turn, dont allow it to give negative power

  5. our PIDVA loop is set to: 3,0,0.5 for the PID, and we adjust VA according to the paths.

  6. We made the turn use a PID loop instead of just P.

Here is the code we are using

    private final double TIME_STEP = .05;

private final double MAX_VELOCITY = .07;
private final double MAX_ACC = .25;
private final double MAX_JERK = 60.0;
private final static double P = 0.0001;
private final static double I = 0.000;
private final static double D = 0.000;
public void initialize() {
System.out.println(“Pathfinder Started”);
Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_QUINTIC, Trajectory.Config.SAMPLES_LOW, TIME_STEP, MAX_VELOCITY, MAX_ACC, MAX_JERK);
left = new DistanceFollower(Pathfinder.readFromCSV(choosenLFile));
right = new DistanceFollower(Pathfinder.readFromCSV(choosenRFile));
left.configurePIDVA(P, I, D, 1 / MAX_VELOCITY, MAX_ACC);
right.configurePIDVA(P, I, D, 1 / MAX_VELOCITY, MAX_ACC);
System.out.println(“Pathfinder Configured”);

public void execute() {

	double l = left.calculate(drive.getLeftDriveEncoderDistance() / 12 );
	double r = right.calculate(drive.getRightDriveEncoderDistance() / 12);

	double gyro_heading = gyro.getAngle();   // Assuming the gyro is giving a value in degrees
	double desired_heading = Pathfinder.r2d(left.getHeading());  // Should also be in degrees

	double angleDifference = Pathfinder.boundHalfDegrees(desired_heading - gyro_heading);
	double turn = 0.4 * (-1.0/80.0) * angleDifference;

	drive.tankDrive(l - turn, r + turn);

public boolean isFinished() {
return (left.isFinished() && right.isFinished() && (angleDifference < 2 && angleDifference > - 2));

Our drive class is converting encoder counts into inches so we dived that by 12 to get feet

Why is your Max velocity and acceleration such low numbers. And try generating the points onboard. When we read from csv it made our bot jiggle alot, onboard generation for some strange reason makes them smooth.

Also your PID values are extremely low, Jacis PID numbers are meant to be quite high, ours is 3,0,0.5

It looks to me like you may have some confusion in the units of your values. Make sure your values are consistent (if you’re using metres, it should be metres, metres/sec, metres/sec^2, etc. Same deal for feet, inches, kilometres, lightyears, AUs, etc). You should also verify that your encoders are reading as expected.

All of our units are in feet and our encoders read as expected