Cómo hacer un programa en basic pro para un elevador ?

he intentado hacer le programa pero hay un falla de desconexion total para volver al estado inicial del programa.
el caso esque al presionar el boton pues la puerta se abre, al abrirses por completo habra un fin de carrera que desactivara el motor de apertura. esperar 5 segundos y despues empezar a cerrar. se tiene un sensor con fototransistor para que cuando se atraviese en las puertas se vuelva a abrir y otra vez los 5 segundos y cerrar. hasta ahi no hay inconveniente sino que cuando la las puertas ya cerraron el motor sigue encendidon y no sé como apagarlo . uso un PIC 16f628A aqui mi programacion
TRISA=15
TRISB=0
PORTB=0
CICLO:
IF PORTA.0=1 OR PORTA.3=1 THEN
GOSUB INICIO
ELSE
PORTB.0=0
GOTO CICLO
ENDIF

INICIO:
RUTINA:
PORTB.0=1
PORTB.1=0
IF PORTA.1=1 THEN
PORTB.0=0
PAUSE 1000
PORTB.1=1
ELSE
GOTO INICIO
ENDIF
GOTO CICLO ...
 
Atrás
Arriba