Trouble with Pathfinder behavior on different paths

We have been able to follow a generated trajectory fairly accurately using Pathfinder, however, when we change the waypoints used for the trajectory, our path following accuracy decreases to the point that it is unpredictable and unusable. Do you guys have any suggestions on how to make our pathfollowing more consistent? We are accessing all of our constants through RobotPreferences, so the constants can be found below.

double MaxVel = 3.0;
double MaxJerk = 60.0;
double MaxAccel = 1.12776;
double GyroP = 0.6;
double P = 0.6;
double I = 0;
double D = 0;

forgot to link code, here is the link to the branch where we are working on our pathfinder-based autos:

What type of paths is it following now? What about the new paths is different? Are the new paths more curved?

We are able to follow the switch path fairly accurately, although our distances aren’t exactly correct. However, when we try to follow the scale path, we undershoot on the scale point, as well as the last point. In addition, the robot turns 180 degrees before moving backwards, instead of maintaining the expected 0 degree heading throughout the path. The scale path is actually less curved, compared to the switch path. The most notable difference seems to be the part of the scale path in which the robot is required to drive backwards in order to maintain it’s desired heading.

The first thing I would do is break the path up into multiple paths. As far as direction, look at the theta of your angles. I think you could also check isReveresed but that would be less clean.

What do you mean by checking isReversed? Also, after graphing our expected heading throughout the scale path, we noticed that the robot expected to be at 180 degrees when it backed up, rather than the specified 0 degrees. Through trial and error, we were able to determine that the last point was the cause of the heading issues. Does pathfinder react badly to paths in which the robot is expected to move backwards? We haven’t tried breaking up the paths yet, which could mean that the strange reaction is a result of something in our path.

It seems like pathfinder may have trouble with “negative” values, as seen on this ( git issue. I would strongly advise breaking the path up, I think that will fix your problem.

for your overall accuracy, have you tried using the I term? this can help correct for steady state errors. It might also indicate you need a hair more P gain, though that’s not always the case.

As you get more and more aggressive with P, you can make faster moves, and then D comes in to dampen the response to not be crazy.

We’re having trouble driving backwards with Pathfinder as well. Did you find a solution?

The ‘I’ term in pathfinder doesn’t do anything. With a continuously changing setpoint, the I term is almost useless.

You should generate your paths as if you were going forwards, and then modify the signal sent to your motors such that it is inverted (1 to -1) and flipped (right to left)

This works for straight paths, but for curved ones the robot doesn’t seem to react as expected. If we run two of the same path one after the other, but invert and flip the signal to one of them, would we expect the robot to end up where it started? Wouldn’t the segments be followed backwards? For example, the first point would need to be the last, and so on.

We are seeing our paths not correct for heading when reversed.