Added some framework for the autonomous code

This work will be merged back into theory, to continue developing
autonomous there