Updated ultrasonic and autonomous code