Измерение расстояния arduino

Написать код к Arduino для измерителя по 3м осям, с помощью ультразвуковых датчиков HC-SR04.

К плате Arduino Leonardo, подключено 3 датчика HC-SR04 и экран 1602 I2C.

Шаг первый: Калибровка.

При включении питания, каждый датчик определяет расстояние до стенки (20 замеров, выводим среднее арифметическое для каждого датчика). В это время на экран выводится надпись Calibrating. После калибровки, надпись в две строки

Calibrating 
complete

Шаг второй: Измерение.

В режиме ожидания выводить:

Ready for
measure

Затем перед датчиками ставится объект, необходимо определить размер объекта перед каждым датчиком в мм учитывая расстояние до стенки в которую смотрит датчик (использовать расстояние которое мы получили в режиме калибровки). Т.е расстояние до стенки - расстояние до объекта = размер объекта

Произвести замер 20 раз, для каждого датчика и вывести среднюю величину (для более точного определения).
В итоге должно получиться три значения. Вывести значения на экран

Size:
X * Y * Z mm

Шаг третий: Передача данных.

 При нажатии на внешнюю кнопку, происходит передача данных по HID интерфейсу в буфер клавиатуры, в формате X (Enter) Y (Enter) Z (Enter) сымитировав ручной ввод данных. После окончания передачи, написать DONE и вернуться к шагу 2.