Switch to pure pursuit for path following

[Ryan4253]

The current TimedTrajectory usage doesn't make much sense since we are not following the profiled velocity and simply using pid to move toward the target point. This means that the time it provides actually wouldn't make much sense in the first place.

It is probably better to use something purely feedback based like pure pursuit to follow discrete waypoints. The main issue associated with pure pursuit, the end position / settle angle wouldn't be much of an issue with holonomic drives, meaning pure pursuit can be very robust for holonomic drives.

[jazonshou]

The reason I have TimedTrajectory is to make the updated Pathplanner compatible with HolonomicLib. It takes the CSV output from Pathplanner and converts it to a TimedTrajectory. Thus, I am not exactly sure why it does not make sense. The only concern with the current approach (which certainly is a legitimate concern) is, that if the chassis hits an obstacle, the controller will continue iterating through the trajectory. This means that instead of driving to the next set point, the chassis will try to drive to the iterated point, which may be much further down in the trajectory.

In other words, I agree with your idea of pure pursuit, but I think the current approach is good for most situations.