Use PathPlanner OTF trajectory generation for drive-to-pose

[nlaverdure]

Generate PathPlanner trajectories on-the-fly and use them for drive-to-pose, instead of the existing PID loops on x, y, theta.

This will allow:

• Adding a correctly-oriented intermediate waypoint prior to encountering the reef, forcing the robot to drive straight into the reef
• Cranking the P gain up very high to overcome friction effects, without increasing the overall speed of the driving