Помогите изменить код

Вывести на дисплей это как раз совсем не сложно. У вас уже есть готовый код. Весь вопрос состоит в том, что-бы преобразовать то, что вы получаете с датчика в то что можно скормить дисплею. Если данное верно

то вам нужно написать счётчик импульсов.

 

Частотомер в помощь

 

всё прекрасно делается на таймере в режиме подсчёта внешних импульсов (capture)