Navigation Improvements: Lock to Path, Intercept Path, Turning

[jkleiber]

We are going to need to know a lot about the path while driving. Specifically we need to be able to follow the path (and return to the path when we deviate)

When we are on the path we need to lock (or couple) to the path with some sort of PID control. We need to be able to unlock (or decouple) when an obstacle is on the path and proceed along an alternate route until we can return to the path.

If we decouple from the path, we can skip some trajectory points but not all of them (bonus cones, the end cone). We therefore need to be able to intercept the original path again, or make a new path to the next required point (online path planning).

This issue seeks to keep the robot locked on the path, and will also include path interception and turning capabilities (bezier curves?). Instructions will be implemented on the motion board, but the code will probably end up in the trajectory folder of the main board

[jkleiber]

I am closing this issue and updating the project with new relevant issues