Updated the Controller code to account for corner cases, it is more robust and will take the optimal path to the desired waypoint