+ ultrasonic sensor functions

Lots of improvements