Added ultrasonic pid driving