Finish implementing ultrasound sensors in firmware and arduino.py