Pathfinder - Fast Motion Profiling for Java and C++, Tank and Swerve Drive

[Ben Wolsieffer]

I just saw your post, and I am probably running into the same problem. I'm a little confused as to how the coordinate system for Pathfinder works.

This is how I assumed it works, but I am probably misunderstanding it:
If the robot starts facing towards the direction I define as forward (ie. towards the opponent's castle):
Its initial heading is pi/2
A waypoint with x = 0, y > 0 and heading = pi/2 will cause the robot to drive straight forward.
A waypoint with x < 0, y = 0 and heading = pi will cause the robot to turn to the left and drive to the left, ending up facing left.
A waypoint with x < 0, y = 0 and heading = 0 will cause the generator to crash.
A waypoint with x = 0, y > 0 and heading = pi/4 will cause the robot to end in a position directly in front of where it started, but pointing to the right.

EDIT: I reread your post, and updated my examples.