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

#include <Ultrasonic.h>
#define IR_sensor_front A0 // front sensor 
#define IR_sensor_back A1 // 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;

void setup() 
{
Serial.begin(9600);

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

pinMode(7, OUTPUT);  // verde 
pinMode(8, OUTPUT);  // amarelo
pinMode(9, OUTPUT);  // azul
pinMode(10, OUTPUT); // vermelho
}

/* 
  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
 int IR_front = (analogRead(IR_sensor_front) * 5) / 1023;
 int IR_back = (analogRead(IR_sensor_back) * 5) / 1023;

 //realiza a leitura da distancia até o obstáculo
 distance = ultrasonic.read();

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

    Stop();

    while (distance < 25 ) //rotina de avanço
    {
      FORWARD(SPEED_MAX); 

      //realiza as leituras mais uma vez para evitar a faixa branca
      distance = ultrasonic.read();
      IR_front = (analogRead(IR_sensor_front) * 5) / 1023;
      IR_back = (analogRead(IR_sensor_back) * 5) / 1023;

      if ( IR_front >= 650) || (IR_back >= 650 ) //se detectar a faixa branca
      { 
        break;
      }
      //delay(10); 
    }
  
  
 if (IR_front < 650 ) 
   {
    Stop();
    delay (50);
    BACKWARD(SPEED_MAX);
    delay (500);
   } 
   
 if (IR_back < 650 ) 
   {
    Stop();
    delay (50);
    FORWARD(255);
    delay (500);
   }
   
 //----------- debug ----------------
  Serial.print("ultrasonic.Ranging(CM): ");
  Serial.println(distance);
  Serial.print("IR front :");
  Serial.println(IR_front); 
  Serial.print("IR back :");
  Serial.println(IR_back);  
 

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


void FORWARD (int Speed) //avança
{
  digitalWrite(7, HIGH); 
  digitalWrite(8, LOW);
  digitalWrite(9, LOW);
  digitalWrite(10, LOW);

  /*analogWrite(IN1,Speed);
  analogWrite(IN2,0);
  analogWrite(IN3,0);
  analogWrite(IN4,Speed);*/
}//--------------------------------------------


void BACKWARD (int Speed) //recua
{
  digitalWrite(7, LOW); 
  digitalWrite(8, HIGH);
  digitalWrite(9, LOW);
  digitalWrite(10, LOW);

  /*analogWrite(IN1,0);
  analogWrite(IN2,Speed);
  analogWrite(IN3,Speed);
  analogWrite(IN4,0);*/
}//--------------------------------------------

void ROTATE (int Speed) //gira 
{
  digitalWrite(7, LOW); 
  digitalWrite(8, LOW);
  digitalWrite(9, HIGH);
  digitalWrite(10, LOW);

 /* analogWrite(IN1,Speed); 
  analogWrite(IN2,0);
  analogWrite(IN3,Speed);
  analogWrite(IN4,0);*/



}//--------------------------------------------
void Stop() //para
{
  digitalWrite(7, LOW); 
  digitalWrite(8, LOW);
  digitalWrite(9, LOW);
  digitalWrite(10, HIGH);

  /*analogWrite(IN1,0);
  analogWrite(IN2,0);
  analogWrite(IN3,0);
  analogWrite(IN4,0);*/
}