add ultrasonic, servo test