Added some autonomous code and stoof