Реализовал ультрасоник через прерывание, теперь он работает асинхронно