Added newly tested autonomous code and new tele-op code