Made many improvements to Autonomous code

Added a case statement making it easier to add additional states
Added the ability to turn
Refined the P controller to allow for changing target speeds