Finish debugging firmware to support ultrasound sensors, still need some work