Added autonomous code

not sure if it works correctly