/* Exemplo de código Robô Sumo em fase de testes
* Ultima versão de 10/04/23
*/

#include <Ultrasonic.h>
#define SNSR_DIR A0 // front sensor 
#define SNSR_ESQ A1 // rear senson
#define SNSR_TRAS A2 // rear senson

#define SPEED_MAX 255

Ultrasonic ultrasonic(4,3);

/*deinição dospínos do motor A e motor B
const int IN1=5;
const int IN2=6;
const int IN3=9;
const int IN4=10;*/


/*
  faixa de tensão no sensor IR:
   cor branca: 2,6V
   cor preta: 4,7V
*/


int distance;

//realiza a leitura de tensão nos pinos A0 e A1
int snsr_dir;
int snsr_esq;
int snsr_tras;
 
void setup() 
{
	Serial.begin(9600);

	pinMode(7, OUTPUT);  // verde 
	pinMode(8, OUTPUT);  // azul claro
	pinMode(9, OUTPUT);  // azul
	pinMode(10, OUTPUT); // vermelho
	pinMode(11, OUTPUT); // laranja
	pinMode(12, OUTPUT); // lilás

	delay (5000); //aguarda 5 segundos para começar a atuar

	
	do
	{
		//faz o primeiro avanço
		FORWARD(SPEED_MAX);
	
	} while ((((analogRead(SNSR_DIR)* 5) / 1023) >= 4.7) ||  //realiza a leitura apenas dos sensores da frente pois frente
				   (((analogRead(SNSR_ESQ)* 5) / 1023) >= 4.7));	//considerando o avanço inicial
	
	Stop(); //faz a primeira parada
	
	delay (500);
	
	for (int i = 0; i < 3; i++) 
	{
		BACKWARD(SPEED_MAX); //recua
		
		if (((analogRead(SNSR_TRAS)* 5) / 1023)	>= 4.7)
		{
			Stop(); //faz a segunda parada caso encontre a faixa no recuo
		}
		
		delay(500);
	}
	Serial.println("loop");
}


	/*
		
		A partir daqui será executado o loop para varredura da área do tatami 
		a procura do oponente a 20 cm ou menos de distancia
		
	*/



/* 	--- Válidação dos sensores IR --- 
  sensor com uma fita
  valor minimo 0.27 a 0.30V ou menos
  valor maximo 4.3, 4.41 e 4.5 ou mais
  
  sensor com duas fitas 
   valor minimo range de 1.5 a 1.7
   valor maximo range de 4.5, 4.6 e 4.7

  sensor com três fitas
   valor minimo range de 1.3 a 1.4
   valor maximo range de 4.5, 4.6 e 4.7
   
 */


void loop()
{
	//realiza a leitura de tensão nos pinos A0 e A1
	snsr_dir  =  (analogRead(SNSR_DIR) * 5) / 1023;
	snsr_esq  = (analogRead(SNSR_ESQ) * 5) / 1023;
	snsr_tras = (analogRead(SNSR_TRAS) * 5) / 1023;

	//faz o giro procurando o adversário
    ROTATE(200); 

	//realiza a leitura da distancia até o obstáculo. valor em cm
	distance = ultrasonic.read();
	
	
	//rotina de avanço procurando o adversário --------------------------------
    while (distance <= 20 ) 
    {
      FORWARD(SPEED_MAX); 

      //realiza as leituras mais uma vez para evitar a faixa branca
      distance = ultrasonic.read();
      snsr_dir = (analogRead(SNSR_DIR) * 5) / 1023;
	    snsr_esq = (analogRead(SNSR_ESQ) * 5) / 1023;
      snsr_tras = (analogRead(SNSR_TRAS) * 5) / 1023;

      if (( snsr_dir <= 2.7) || (snsr_esq <= 2.7)) //se encontrar a faixa na parte frontal recua 
      { 
        BACKWARD(SPEED_MAX); //recua
      }
	  else if (snsr_tras <= 2.7) // se encontar a faixa na parte traseira
	  {
		  FORWARD(SPEED_MAX); //avança
	  }
      
    }
	// -----------------------------------------------------------------------
  
  
	if (snsr_dir <= 2.7) //se houver detecção no sensor da direita
	{
		Stop();
		delay (50);
		BACKWARD(SPEED_MAX); //recua 
		esq(SPEED_MAX);
		delay (500);
	} 
   
	if (snsr_esq <= 2.7) //se houver detecção no sensor da esquerda
	{
		Stop();
		delay (50);
		BACKWARD(SPEED_MAX); //recua 
		dir(SPEED_MAX);
		delay (500);
	} 
   
	if (snsr_tras <= 2.3) //se houver detecção no sensor da traseira
	{
		Stop();
		delay (50);
		FORWARD(SPEED_MAX); //avança
		delay (500);
	}
   FORWARD(SPEED_MAX); //avança
 //----------- debug ----------------
  Serial.print("distancia: ");
  Serial.println(distance);
   
 

} //--------------------------------------------


void FORWARD (int Speed) //avança
{
  digitalWrite(7, HIGH); // verde 
  digitalWrite(8, LOW);  // azul claro
  digitalWrite(9, LOW);  // azul
  digitalWrite(10, LOW); // vermelho
  digitalWrite(11, LOW); // laranja
  digitalWrite(12, LOW); // lilás
	Serial.println(" - Avaca");
  /*analogWrite(IN1,Speed);
  analogWrite(IN2,0);
  analogWrite(IN3,0);
  analogWrite(IN4,Speed);*/
}//--------------------------------------------


void BACKWARD (int Speed) //recua
{
  digitalWrite(7, LOW);  // verde 
  digitalWrite(8, HIGH); // azul claro
  digitalWrite(9, LOW);  // azul
  digitalWrite(10, LOW); // vermelho
  digitalWrite(11, LOW); // laranja
  digitalWrite(12, LOW); // lilás
	Serial.println(" - Recua");
  /*analogWrite(IN1,0);
  analogWrite(IN2,Speed);
  analogWrite(IN3,Speed);
  analogWrite(IN4,0);*/
}//--------------------------------------------

void ROTATE (int Speed) //gira 
{
  digitalWrite(7, LOW);  // verde 
  digitalWrite(8, LOW);  // azul claro
  digitalWrite(9, HIGH); // azul
  digitalWrite(10, LOW); // vermelho
  digitalWrite(11, LOW); // laranja
  digitalWrite(12, LOW); // lilás

Serial.println(" - Gira");
 /* analogWrite(IN1,Speed); 
  analogWrite(IN2,0);
  analogWrite(IN3,Speed);
  analogWrite(IN4,0);*/



}//--------------------------------------------
void Stop() //para
{
  digitalWrite(7, LOW);   // verde 
  digitalWrite(8, LOW);   // azul claro
  digitalWrite(9, LOW);   // azul
  digitalWrite(10, HIGH); // vermelho
  digitalWrite(11, LOW); // laranja
  digitalWrite(12, LOW); // lilás
	Serial.println(" - PARA");
  /*analogWrite(IN1,0);
  analogWrite(IN2,0);
  analogWrite(IN3,0);
  analogWrite(IN4,0);*/
}

void dir(int Speed)
{
	
	digitalWrite(7, LOW);  // verde 
  digitalWrite(8, LOW);  // azul claro
  digitalWrite(9, LOW); // azul
  digitalWrite(10, LOW); // vermelho
  digitalWrite(11, HIGH); // laranja
  digitalWrite(12, LOW); // lilás
  
  Serial.println(" - Direita");
  
  /*analogWrite(IN1,0); 
  analogWrite(IN2,Speed);
  analogWrite(IN3,0);
  analogWrite(IN4,Speed);*/
}

void esq(int Speed)
{
  digitalWrite(7, LOW);  // verde 
  digitalWrite(8, LOW);  // azul claro
  digitalWrite(9, LOW); // azul
  digitalWrite(10, LOW); // vermelho
  digitalWrite(11, LOW); // laranja
  digitalWrite(12, HIGH); // lilás
  Serial.print(" - Esquerdaln");
  
  /*analogWrite(IN1,Speed); 
  analogWrite(IN2,0);
  analogWrite(IN3,Speed);
  analogWrite(IN4,0);*/
}