Added init

Added init from corey's ultrasonic.c file