Autonomous and Merge

Merges the current versions of the code and adds to autonomous mode.