Added working tele-op and more auto mission 1