Updated autonomous code

TiffanyR works. TiffanyL untested code