Firstly, my experience with VEX Ultrasonic Sensors showed that they had a bit of “noise”, so perhaps you could take the average of the past 5 values or so in order to get a more accurate reading. Also, if you want, I would look into creating some functions for driving in autonomous, so you don’t have to copy/paste code. Perhaps one function for driving using IMEs, one for driving using Ultrasonics, and another for turning using the gyro.
~Jordan