Basic autonomous outline, and code

Added functions as an outline for the autonomous method take a look