Fix bugs, implement features, starting implementation of ultrasonicsensor