V4.1  Re-Did Final run algo

-Made a cross function that makes the navigation more modular
-Made a reorient function that will turn the robot to a given direction