Autonomous done, changed program structure

Everything is accounted for in the code.
However, there are a few guessed numbers in the code that need tested.