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
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: