Función para leer sensor ultrasónico

Hola, esta función lee un sensor ultrasónico y entrega un verdadero al programa que la invoca cuando el objeto sensado permanece por más de 5 segundos en su posición. Para ello implementé el cont_1. También implementé un cont_2 para evitar pérdida de sensado si por ejemplo por cortos intervalos de tiempo el sensor pierde de vista al objeto por un movimiento involuntario por ejemplo. Me funciona bien esta segunda parte pero no la primera. Es como que la función devuelve un verdadero ni bien entra al if(DISTANCIA>=.... y no me doy cuenta porqué. Aquí se encuentra el código completo pero basta con mirar solo la función “presencia” https://pastebin.com/Q1jwFFrm

Moderador
Por favor edita tu post usando etiquetas para que se vea adecuadamente el enlace. Consulta abajo cómo se hace.
Los códigos es mejor que se publiquen aquí directamente.

Normas del foro