Finished Autonomous code

This should be the last update to Autonomous before the competition. In
theory, this will work for all scenarios.