I am having issues with how my path planner is rotating the robot. The robot drives exactly how it should when there is a straight path or even a translating path with no rotating involved. It is when I attempt to add a rotation aspect to it that it goes off of its path at the end. A picture of the intended path and a video of what actually happens are included as well as our Code Repository. Any help is greatly appreciated.
Actual Output