/*este é o programa utilizado para controlar o motor do robô */
/* O codigo poderia ter sido otimizado porem tivemos apenas 1 semana para escrever este codigo poucas horas por dia devido a um problema no cronograma do projeto Cybercup. Temos ciencia que este codigo poderia ser muito melhor */
#include <reg52.h>

at 0xB5 sbit en1; 
at 0xB6 sbit rd1; 
at 0xB7 sbit re1; 

at 0xB2 sbit en2;
at 0xB3 sbit rd2;
at 0xB4 sbit re2;

at 0x90 sbit LED_VD_1;
at 0x91 sbit LED_AM_1;
at 0x92 sbit LED_VM_1;

at 0xA3 sbit SONAR_FRONTAL;  
at 0xA2 sbit SONAR_ESQUERDA;   
at 0xA7 sbit SONAR_DIREITA;      

int DISTFRONTAL,DISTESQUERDA,DISTDIREITA;
int SONARL, SONARA, SONARB;

void delay_correcao4(void){ 
	int o;
	float p;
	TMOD = 0x01;
	TH0 = 0xDC;
	TL0 = 0x00;
	TR0 = 1;
	for(o=0;o<2;o++){
   		p = 0;
   		while(p<1){		
   								
   				while(!TF0);
   				TH0=0xB7;   
							// 170 foi pra direita ..
							// 160 foi mto para direita
							// 185 foi quase reto.. foi um pouquinho pra direita
							// 190 bb foi pra direita.
							// 183
   				TL0=0x00;  
   				TF0=0; 
   				p=p+1;
   		}    
   }
}	

void delay_1s(void){
  int i,j;
	TMOD = 0x01;
	TH0 = 0xDC;
	TL0 = 0x00;
	TR0 = 1;
  for(i=0;i<1;i++){		
      j = 0;
      while(j<=100){		
          while(!TF0);
          TH0=0xDC;
          TL0=0x00;
          TF0=0;
          j++;
      }    
   }
}

void virar_esquerda(void){
	en1=1;
	en2=1;		
	rd1 = 0;
	re1 = 1;
	rd2 = 1;
	re2 = 0;
}

void virar_direita(void){
	en1=1;
	en2=1;		
	rd1 = 1;
	re1 = 0;
	rd2 = 0;
	re2 = 1;
}

void delay_05s(void){
	int o,p;
	TMOD = 0x01;
	TH0 = 0xDC;
	TL0 = 0x00;
	TR0 = 1;
	for(o=0;o<2;o++){
   		p = 0;
   		while(p<=8){
   				while(!TF0);
   				TH0=0xDC;
   				TL0=0x00;
   				TF0=0;
   				p++;
   		}    
   }
}	


void reto(void){

		en1=1;
		en2=1;
		rd1 = 0;
		re1 = 1;			
		rd2 = 0;
		re2 = 1;			
		en1=0;
		delay_correcao4();   // en1=direita
		en1=1;

			
}

void parar(void){
	en1=0;
	en2=0;
	}	
	
void re(void){
	en1=1;
	en2=1;		
	rd1 = 1;
	re1 = 0;
	rd2 = 1;
	re2 = 0;
}



void delay_curvaesquerda(void){ 
	float o,p;
	TMOD = 0x01;
	TH0 = 0xDC;
	TL0 = 0x00;
	TR0 = 1;
	for(o=0;o<2;o++){
   		p = 0;
   		while(p<4){	
   				while(!TF0);  	
   				TH0=0xBD;    
   				TL0=0x00;
   				TF0=0;
   				p++;   				
   		}    
   }
}

void delay_curvadireita(void){ 
	float o,p;
	TMOD = 0x01;
	TH0 = 0xDC;
	TL0 = 0x00;
	TR0 = 1;
	for(o=0;o<2;o++){
   		p = 0;
   		while(p<6){  
			while(!TF0);
			TH0=0xD7;    
			TL0=0x00;	
							
			TF0=0;
			p++;
   		}    
	}
}

void delay_re(void){  //NAO MEXER NO DELAY
	int o,p;
	TMOD = 0x01;
	TH0 = 0xDC;
	TL0 = 0x00;
	TR0 = 1;
	for(o=0;o<2;o++){
   		p = 0;
   		while(p<=9){
   				while(!TF0);
   				TH0=0xDC;
   				TL0=0x00;
   				TF0=0;
   				p++;
   		}    
   }
}	

void delay_pancada(void){  //NAO MEXER NO DELAY
	int o,p;
	TMOD = 0x01;
	TH0 = 0xDC;
	TL0 = 0x00;
	TR0 = 1;
	for(o=0;o<2;o++){
   		p = 0;
   		while(p<=10){ // 10
   				while(!TF0);
   				TH0=0xDC;
   				TL0=0x00;
   				TF0=0;
   				p++;
   		}    
   }
}	

void debuga(int delay){
	int i;
	for(i=0;i<delay;i++){ delay_1s(); }
}

void SENSOR_FRONTAL(void){
			DISTFRONTAL = 0;
			while(SONAR_FRONTAL == 0){ }
			while(SONAR_FRONTAL == 1){ }
			while(SONAR_FRONTAL == 0){ }
			while(SONAR_FRONTAL == 1){
				DISTFRONTAL = DISTFRONTAL + 1;
 				for(SONARL=0;SONARL<1;SONARL++){ }
      	}   	
}

void SENSOR_ESQUERDA(void){
			DISTESQUERDA = 0;
			while(SONAR_ESQUERDA == 0){ }
			while(SONAR_ESQUERDA == 1){ }
			while(SONAR_ESQUERDA == 0){ }
			while(SONAR_ESQUERDA == 1){
				DISTESQUERDA = DISTESQUERDA + 1;
 				for(SONARA=0;SONARA<1;SONARA++){ }
      	}   	
}

void SENSOR_DIREITA(void){
			DISTDIREITA = 0;
			while(SONAR_DIREITA == 0){ }
			while(SONAR_DIREITA == 1){ }
			while(SONAR_DIREITA == 0){ }
			while(SONAR_DIREITA == 1){
				DISTDIREITA = DISTDIREITA + 1;
 				for(SONARB=0;SONARB<1;SONARB++){ }
      	}   	
}

void main(void){
	
	// int primeirareta;
	
	SONAR_FRONTAL = 1;	
	SONAR_DIREITA = 1;
	SONAR_ESQUERDA = 1;
																		
	LED_VD_1 = 1;							
	LED_AM_1 = 1;							
	LED_VM_1 = 1;							
	en1 = 1;									
	en2 = 1;									

	delay_05s();

	while(1)	{	

		int j=0;

	   SENSOR_FRONTAL();
	   
		if (DISTFRONTAL <40){  
		
			en1=0;
			en2=0;


			SENSOR_DIREITA();
			SENSOR_ESQUERDA();	

			while(j<5){
				if(DISTDIREITA > 50 && DISTESQUERDA > 50){
					SENSOR_DIREITA();
					SENSOR_ESQUERDA();
				}
				else j=5;
				
				 j=j+1;
				
			}
			
			if(DISTDIREITA < DISTESQUERDA){     // estava 40
				reto(); delay_pancada();
				parar();
				re(); delay_re(); 
				parar();
				delay_05s();
				virar_esquerda();
				delay_curvaesquerda();
				parar();
				delay_05s();
				
			}		
			else if(DISTESQUERDA < DISTDIREITA){      // estava 40
				reto(); delay_pancada();
				parar();					
				re(); delay_re(); 
				parar();
				delay_05s();
				virar_direita();
				delay_curvadireita();
				parar();
				delay_05s();
			}
			
		}
		//else if (DISTFRONTAL <= 30){ re(); delay_re(); }
		else { reto(); }		  
   } 		
}