Finished the hardcoded Autonomous

This will allow us to pick which beacon we go to in Autonomous mode.