Fixed Ultrasonic, modify logger
Merged code to Ben's code