#include <16F873a.h>
 #FUSES XT,NOWDT,NOPROTECT,NOPUT
 #use delay(clock=4000000)
         
 int16 distanciaf, tiempof;
 int16 distanciat, tiempot;
 int16 distanciad, tiempod;
 int16 distanciai, tiempoi;
 
 #define disparof PIN_C0
 #define disparot PIN_C1
 #define disparod PIN_C2
 #define disparoi PIN_C3
 #define ecof PIN_C4
 #define ecot PIN_C5
 #define ecod PIN_C6
 #define ecoi PIN_C7

 #BYTE TRIS_A = 0X85
 #BYTE PORT_A = 0X05
 #BYTE TRIS_B = 0X86
 #BYTE PORT_B = 0X06
 #BYTE TRIS_C = 0X87
 #BYTE PORT_C = 0X07
 #use fast_io(B)

#INT_RB 
void RB_isr(){ 
	unsigned int8 valor=input_B();
	valor=valor&0b11110000;
	switch(valor){
		case 0b00010000:
 	 		output_A(0b111010);
    		break;
 
		case 0b00100000: 
			output_A(0b110101);
	   		break;
		
		case 0b01000000:
			output_A(0b111001);
			break;			 
	
		case 0b10000000:
			output_A(0b110110);
			break;
 		default:
 			break;
}
 clear_interrupt(INT_RB);
} 


 void main()
 {
 setup_adc_ports(NO_ANALOGS);
 clear_interrupt(INT_RB);
 enable_interrupts(INT_RB); 
 enable_interrupts(GLOBAL);
 set_tris_A(0x00);
 set_tris_B(0xF0);
 set_tris_C(0b11110000);
 
 PORT_A=0;
 PORT_B=0;
 PORT_C=0;



 delay_ms(5000);
 output_A(0b110101);
 delay_ms(4500);
          

 while(1)
 {
lectura:
 setup_timer_1(T1_INTERNAL|T1_DIV_BY_8);	 
 
 //Sensor adelante
 output_high(disparof);                   
 delay_us(20);                       
 output_low(disparof);
 while(!input(ecof))                     
 {
 set_timer1(0);
 }                        
 while(input(ecof))                     
 {
 tiempof=get_timer1();
 distanciaf=(tiempof*10)/(58.0);
 delay_ms(50);
 break;
 }
 
 //Sensor atras
 output_high(disparot);                   
 delay_us(20);                       
 output_low(disparot);
 while(!input(ecot))                     
 {
 set_timer1(0);
 }                         
 while(input(ecot))                     
 {
 tiempot=get_timer1();
 distanciat=(tiempot*10)/(58.0);
 delay_ms(50);
 break;
 }
 
 //Sensor derecho
 output_high(disparod);                   
 delay_us(20);                       
 output_low(disparod);
 while(!input(ecod))                     
 {
 set_timer1(0);
 }                         
 while(input(ecod))                     
 {
 tiempod=get_timer1();
 distanciad=(tiempod*10)/(58.0);
 delay_ms(50);
 break;
 }
 
 //Sensor izquierdo
 output_high(disparoi);                   
 delay_us(20);                       
 output_low(disparoi);
 while(!input(ecoi))                     
 {
 set_timer1(0);
 }                         
 while(input(ecoi))                     
 {
 tiempoi=get_timer1();
 distanciai=(tiempoi*10)/(58.0);
 delay_ms(50);
 break;
 }
    
 if(distanciaf>50 && distanciat>50 && distanciad>50 && distanciai>50){ // si lectura > 50cm entonces no es valida
 	goto lectura;
   }
 else{ // si distancia fue < 50cm es una distancia valida
      if(distanciaf<distanciat && distanciaf<distanciad && distanciaf<distanciai){
         output_A(0b110101);
			goto lectura;
 	  }
 	  
 	  if(distanciat<distanciaf && distanciat<distanciad && distanciat<distanciai){
         output_A(0b111010);
			goto lectura;
      }   
      
 	  if(distanciad<distanciaf && distanciad<distanciat && distanciad<distanciai){
         output_A(0b111001);
         goto lectura;
      } 
      
	  if(distanciai<distanciaf && distanciai<distanciat && distanciai<distanciad){
         output_A(0b110110);
         goto lectura;
      }
 }
 }
 }