#include <IRremote.h>  
#include <Servo.h>
// *********************** Definicion de pines de motores *************************
int MotorRight1=5;
int MotorRight2=6;
int MotorLeft1=10;
int MotorLeft2=11;
int counter=0;
const int irReceiverPin = 2; // La salida OUTPUT del receptor IR va conectada al pin 2

// *********************** Codigos IR detectados *************************
long IRfront= 0x00FFA25D;        // Codigo para avanzar
long IRback=0x00FF629D;         // Retroceder
long IRturnright=0x00FFC23D;    // Girar a la derecha
long IRturnleft= 0x00FF02FD;     // Girar a la izquierda
long IRstop=0x00FFE21D;         // Detener
long IRcny70=0x00FFA857;        // Modo autonomo CNY70
long IRAutorun=0x00FF906F;      // Modo autonomo con ultrasonico
long IRturnsmallleft= 0x00FF22DD; 
// ************************* Pines de sensores CNY70 ************************************
const int SensorLeft = 7;      // Entrada del sensor izquierdo
const int SensorMiddle= 4 ;    // Entrada del sensor central
const int SensorRight = 3;     // Entrada del sensor derecho
int SL;    // Estado del sensor izquierdo
int SM;    // Estado del sensor central
int SR;    // Estado del sensor derecho
IRrecv irrecv(irReceiverPin);  // Objeto IRrecv para recibir senales IR
decode_results results;       // El resultado de decodificacion se guarda en la estructura decode_results
// ************************* Pines del sensor ultrasonico ******************************
int inputPin =13 ; // ECHO del sensor ultrasonico HC-SR04
int outputPin =12; // TRIG del sensor ultrasonico HC-SR04
int Fspeedd = 0; // Distancia al frente
int Rspeedd = 0; // Distancia a la derecha
int Lspeedd = 0; // Distancia a la izquierda
int directionn = 0; // Direccion: frente=8, atras=2, izquierda=4, derecha=6
Servo myservo; // Declara el servo myservo
int delay_time = 250; // Tiempo de estabilizacion despues de girar el servo
int Fgo = 8; // Avanzar
int Rgo = 6; // Girar a la derecha
int Lgo = 4; // Girar a la izquierda
int Bgo = 2; // Reversa
// ********************************************************************(SETUP)
void setup()
{  
  Serial.begin(9600);
  pinMode(MotorRight1, OUTPUT);  // D5 PWM
  pinMode(MotorRight2, OUTPUT);  // D6 PWM
  pinMode(MotorLeft1,  OUTPUT);  // D10 PWM
  pinMode(MotorLeft2,  OUTPUT);  // D11 PWM
  irrecv.enableIRIn();     // Inicia la decodificacion IR
   pinMode(SensorLeft, INPUT); // Configura el sensor izquierdo
  pinMode(SensorMiddle, INPUT);// Configura el sensor central
  pinMode(SensorRight, INPUT); // Configura el sensor derecho
  digitalWrite(2,HIGH);
  pinMode(inputPin, INPUT); // Configura el pin de entrada ultrasonico
  pinMode(outputPin, OUTPUT); // Configura el pin de salida ultrasonico
  myservo.attach(9); // Configura el pin PWM del servomotor


 }
// ******************************************************************(Funciones)
void advance(int a) // Avanzar
{
        digitalWrite(MotorRight1,LOW);
        digitalWrite(MotorRight2,HIGH);
        digitalWrite(MotorLeft1,LOW);
        digitalWrite(MotorLeft2,HIGH);
        delay(a * 100); 
}
void right(int b) // Giro a la derecha con una rueda
{
       digitalWrite(MotorLeft1,LOW);
       digitalWrite(MotorLeft2,HIGH);
       digitalWrite(MotorRight1,LOW);
       digitalWrite(MotorRight2,LOW);
       delay(b * 100);
}
void left(int c) // Giro a la izquierda con una rueda
{
      digitalWrite(MotorRight1,LOW);
      digitalWrite(MotorRight2,HIGH);
      digitalWrite(MotorLeft1,LOW);
      digitalWrite(MotorLeft2,LOW);
      delay(c * 100);
}
void turnR(int d) // Giro a la derecha con dos ruedas
{
      digitalWrite(MotorRight1,HIGH);
      digitalWrite(MotorRight2,LOW);
      digitalWrite(MotorLeft1,LOW);
      digitalWrite(MotorLeft2,HIGH);
      delay(d * 100);
}
void turnL(int e) // Giro a la izquierda con dos ruedas
{
      digitalWrite(MotorRight1,LOW);
      digitalWrite(MotorRight2,HIGH);
      digitalWrite(MotorLeft1,HIGH);
      digitalWrite(MotorLeft2,LOW);
      delay(e * 100);
} 
void stopp(int f) // Detener
{
     digitalWrite(MotorRight1,LOW);
     digitalWrite(MotorRight2,LOW);
     digitalWrite(MotorLeft1,LOW);
     digitalWrite(MotorLeft2,LOW);
     delay(f * 100);
}
void back(int g) // Retroceder
{
        digitalWrite(MotorRight1,HIGH);
        digitalWrite(MotorRight2,LOW);
        digitalWrite(MotorLeft1,HIGH);
        digitalWrite(MotorLeft2,LOW);;
        delay(g * 100); 
}
void detection() // Mide tres direcciones: frente, izquierda y derecha
{ 
    int delay_time = 250; // Tiempo de estabilizacion despues de girar el servo
    ask_pin_F(); // Lee la distancia al frente

    if(Fspeedd < 10) // Si la distancia al frente es menor que 10 cm
   {
      stopp(1); // Limpia la salida de motores
      back(2); // Retrocede 0.2 segundos
   }
    if(Fspeedd < 25) // Si la distancia al frente es menor que 25 cm
   {
      stopp(1); // Limpia la salida de motores
      ask_pin_L(); // Lee la distancia a la izquierda
      delay(delay_time); // Espera a que el servo se estabilice
      ask_pin_R(); // Lee la distancia a la derecha
      delay(delay_time); // Espera a que el servo se estabilice

      if(Lspeedd > Rspeedd) // Si la distancia izquierda es mayor que la derecha
     {
        directionn = Lgo; // Se mueve a la izquierda
     }

      if(Lspeedd <= Rspeedd) // Si la distancia izquierda es menor o igual que la derecha
      {
        directionn = Rgo; // Se mueve a la derecha
      } 

      if (Lspeedd < 15 && Rspeedd < 15) // Si izquierda y derecha son menores que 10 cm
     {
        directionn = Bgo; // Se mueve hacia atras
      } 
    }
    else // Si al frente hay mas de 25 cm
   {
      directionn = Fgo; // Se mueve hacia adelante
   }
}   
// *********************************************************************************
void ask_pin_F() // Mide la distancia frontal
{
myservo.write(90);
digitalWrite(outputPin, LOW); // Mantiene Trig en bajo durante 2 us
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // Mantiene Trig en alto durante 10 us como minimo
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // Regresa Trig a nivel bajo
float Fdistance = pulseIn(inputPin, HIGH); // Lee la diferencia de tiempo
Fdistance= Fdistance/5.8/10; // Convierte tiempo a distancia en cm
Serial.print("F distance:"); // Imprime la distancia en cm
Serial.println(Fdistance); // Muestra la distancia
Fspeedd = Fdistance; // Guarda la distancia en Fspeedd
} 
// ********************************************************************************
void ask_pin_L() // Mide la distancia izquierda
{
myservo.write(177);
delay(delay_time);
digitalWrite(outputPin, LOW); // Mantiene Trig en bajo durante 2 us
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // Mantiene Trig en alto durante 10 us como minimo
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // Regresa Trig a nivel bajo
float Ldistance = pulseIn(inputPin, HIGH); // Lee la diferencia de tiempo
Ldistance= Ldistance/5.8/10; // Convierte tiempo a distancia en cm
Serial.print("L distance:"); // Imprime la distancia en cm
Serial.println(Ldistance); // Muestra la distancia
Lspeedd = Ldistance; // Guarda la distancia en Lspeedd
} 
// ******************************************************************************
void ask_pin_R() // Mide la distancia derecha
{
myservo.write(5);
delay(delay_time);
digitalWrite(outputPin, LOW); // Mantiene Trig en bajo durante 2 us
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // Mantiene Trig en alto durante 10 us como minimo
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // Regresa Trig a nivel bajo
float Rdistance = pulseIn(inputPin, HIGH); // Lee la diferencia de tiempo
Rdistance= Rdistance/5.8/10; // Convierte tiempo a distancia en cm
Serial.print("R distance:"); // Imprime la distancia en cm
Serial.println(Rdistance); // Muestra la distancia
Rspeedd = Rdistance; // Guarda la distancia en Rspeedd
} 
// ******************************************************************************(LOOP)
void loop() 
{
      SL = digitalRead(SensorLeft);
      SM = digitalRead(SensorMiddle);
      SR = digitalRead(SensorRight);
// *************************************************************************** Modo normal con control remoto
  if (irrecv.decode(&results)) 
    {         // Decodificacion correcta: se recibio una senal IR
/***********************************************************************/
      if (results.value == IRfront)// Avanzar
       {
        advance(10);// Avanzar
       }
/***********************************************************************/ 
      if (results.value ==  IRback)// Retroceder
       {
        back(10);// Retroceder
       }
/***********************************************************************/
      if (results.value == IRturnright)// Girar a la derecha
      {
        right(6); // Girar a la derecha
      }
/***********************************************************************/
     if (results.value == IRturnleft)// Girar a la izquierda
     {
       left(6); // Girar a la izquierda
     }
/***********************************************************************/    
    if (results.value == IRstop)// Detener
   {
     digitalWrite(MotorRight1,LOW);
     digitalWrite(MotorRight2,LOW);
     digitalWrite(MotorLeft1,LOW);
     digitalWrite(MotorLeft2,LOW);
    }
// *********************************************************************** Modo autonomo CNY70: negro=LOW, blanco=HIGH
    if (results.value == IRcny70)
   {                     
     while(IRcny70)
     {  
       SL = digitalRead(SensorLeft);
       SM = digitalRead(SensorMiddle);
       SR = digitalRead(SensorRight);
                   
        if (SM == HIGH)// El sensor central esta sobre blanco
       { 
          if (SL == LOW & SR == HIGH) // Izquierda negro y derecha blanco: gira a la izquierda
          {  
             digitalWrite(MotorRight1,LOW);
             digitalWrite(MotorRight2,HIGH);
             analogWrite(MotorLeft1,0);
             analogWrite(MotorLeft2,80);
          } 
          else if (SR == LOW & SL == HIGH) // Izquierda blanco y derecha negro: gira a la derecha
          {  
             analogWrite(MotorRight1,0);// Girar a la derecha
             analogWrite(MotorRight2,80);
             digitalWrite(MotorLeft1,LOW);
             digitalWrite(MotorLeft2,HIGH);
          }
         else  // Ambos lados en blanco: avanza
          { 
             digitalWrite(MotorRight1,LOW);
             digitalWrite(MotorRight2,HIGH);
             digitalWrite(MotorLeft1,LOW);
             digitalWrite(MotorLeft2,HIGH);
             analogWrite(MotorLeft1,200);
             analogWrite(MotorLeft2,200);
             analogWrite(MotorRight1,200);
             analogWrite(MotorRight2,200);
         }      
       } 
        else // El sensor central esta sobre negro
      {  
         if (SL == LOW & SR == HIGH)// Izquierda negro y derecha blanco: giro rapido a la izquierda
        {  
            digitalWrite(MotorRight1,LOW);
            digitalWrite(MotorRight2,HIGH);
            digitalWrite(MotorLeft1,LOW);
            digitalWrite(MotorLeft2,LOW);
        }
         else if (SR == LOW & SL == HIGH) // Izquierda blanco y derecha negro: giro rapido a la derecha
        {  
           digitalWrite(MotorRight1,LOW);
           digitalWrite(MotorRight2,LOW);
           digitalWrite(MotorLeft1,LOW);
           digitalWrite(MotorLeft2,HIGH);
        }
          else // Lectura centrada o sin correccion lateral: ajuste hacia atras
        {    
        digitalWrite(MotorRight1,HIGH);
        digitalWrite(MotorRight2,LOW);
        digitalWrite(MotorLeft1,HIGH);
        digitalWrite(MotorLeft2,LOW);;
        }
      }
       if (irrecv.decode(&results))
       {
             irrecv.resume(); 
                  Serial.println(results.value,HEX);
             if(results.value ==IRstop)
             { 
               digitalWrite(MotorRight1,HIGH);
               digitalWrite(MotorRight2,HIGH);
               digitalWrite(MotorLeft1,HIGH);
               digitalWrite(MotorLeft2,HIGH);
               break;
             }
       }
     }
      results.value=0;
   }
// *********************************************************************** Modo autonomo con ultrasonico
 if (results.value ==IRAutorun )
      {
           while(IRAutorun)
        {
            myservo.write(90); // Regresa el servo a la posicion inicial para la siguiente medicion
            detection(); // Mide angulos y decide hacia donde moverse
             if(directionn == 8) // Si directionn = 8, avanzar
            { 
              if (irrecv.decode(&results))
           {
             irrecv.resume(); 
             Serial.println(results.value,HEX);
             if(results.value ==IRstop)
             { 
               digitalWrite(MotorRight1,LOW); 
               digitalWrite(MotorRight2,LOW);
               digitalWrite(MotorLeft1,LOW);
               digitalWrite(MotorLeft2,LOW);
               break;
             }
           }
                results.value=0;
                advance(1); // Avance normal
                Serial.print(" Advance "); // Muestra direccion: avanzar
                Serial.print(" "); 
            }
           if(directionn == 2) // Si directionn = 2, reversa
          {
            if (irrecv.decode(&results))
           {
             irrecv.resume(); 
             Serial.println(results.value,HEX);
             if(results.value ==IRstop)
             { 
               digitalWrite(MotorRight1,LOW); 
               digitalWrite(MotorRight2,LOW);
               digitalWrite(MotorLeft1,LOW);
               digitalWrite(MotorLeft2,LOW);
               break;
             }
           }
              results.value=0;
              back(8); // Retrocede
              turnL(3); // Se mueve un poco a la izquierda para evitar atorarse
              Serial.print(" Reverse "); // Muestra direccion: reversa
          }
            if(directionn == 6) // Si directionn = 6, girar a la derecha
          {
           if (irrecv.decode(&results))
           {
              irrecv.resume(); 
              Serial.println(results.value,HEX);
             if(results.value ==IRstop)
             { 
               digitalWrite(MotorRight1,LOW); 
               digitalWrite(MotorRight2,LOW);
               digitalWrite(MotorLeft1,LOW);
               digitalWrite(MotorLeft2,LOW);
               break;
             }
           }
             results.value=0;
               back(1); 
               turnR(6); // Girar a la derecha
                Serial.print(" Right "); // Muestra direccion: derecha
          }
            if(directionn == 4) // Si directionn = 4, girar a la izquierda
          { 
             if (irrecv.decode(&results))
           {
             irrecv.resume(); 
             Serial.println(results.value,HEX);
             if(results.value ==IRstop)
             { 
               digitalWrite(MotorRight1,LOW); 
               digitalWrite(MotorRight2,LOW);
               digitalWrite(MotorLeft1,LOW);
               digitalWrite(MotorLeft2,LOW);
               break;
             }
           }
                results.value=0;
                back(1); 
                turnL(6); // Girar a la izquierda
                 Serial.print(" Left "); // Muestra direccion: izquierda
           } 
            
             if (irrecv.decode(&results))
           {
             irrecv.resume(); 
             Serial.println(results.value,HEX);
             if(results.value ==IRstop)
             { 
               digitalWrite(MotorRight1,LOW); 
               digitalWrite(MotorRight2,LOW);
               digitalWrite(MotorLeft1,LOW);
               digitalWrite(MotorLeft2,LOW);
               break;
             }
           }
         }
               results.value=0;
       }
/***********************************************************************/    
     else
    {
           digitalWrite(MotorRight1,LOW);
           digitalWrite(MotorRight2,LOW);
           digitalWrite(MotorLeft1,LOW);
           digitalWrite(MotorLeft2,LOW);
     }
      

        irrecv.resume();    // Continua recibiendo la siguiente senal IR
   }  
}
   
