///////////////////////////////////////////////
// * 2 - DEFINICIONES Y VARIABLES A UTILIZAR //                                                                                 .
///////////////////////////////////////////////
// CONTROL REMOTO (IR)  CAR MP3
#define RECV_PIN 12       // 12 Pin receptor de control remoto por infrarrojos
IRrecv irrecv(RECV_PIN);  // Define un control remoto infrarrojo - pin 12
decode_results results;   // Memoria intermedia de código clave de controlador remoto por infrarrojos
#define IR_Go 0x00ff629d            // Adelante "CH"
#define IR_Back 0x00ffa857          // Atras "VOL+"
#define IR_Left 0x00ff22dd          // Gire a la izquierda "PREV"
#define IR_Right 0x00ffc23d         // Gire a la derecha "PLAY/PAUSE"
#define IR_Stop 0x00ff02fd          // Detener - "NEXT"
#define IR_ESC 0x00ff52ad           // Introducción del control remoto "9"
#define IR_Servo_L 0x00ff6897       // Girar a la izquierda del aparato de gobierno "0"
#define IR_Servo_R 0x00ffb04f       // Girar a la derecha del timón (SeUltra)"200+"
#define IR_Utr_Ran 0x00ff9867       // Alcance ultrasónica "100+"
#define IR_Speed_UP 0x00ff7a85      // Aumento de la velocidad "3"
#define IR_Speed_DOWN 0x00ff30cf    // Disminucion de la velocidad "1"
#define IR_FoLine_Mode 0x00ff18e7    // Encender el modo de seguimiento linea "2"
#define IR_AvObj_Mode 0x00ff4ab5  // Encender el modo de evitación de obstáculos "8"
// Módulos opticos para seguimiento
#define SensorLeft 6    // Definir Pin(6)  sensor módulo de seguimiento izquierda
#define SensorMiddle 9  // Definir Pin(9)  sensor módulo de seguimiento central
#define SensorRight 11  // Definir Pin(11) sensor módulo de seguimiento derecho
unsigned char SL; // Variable del estado del Sensor de seguimiento izquierdo
unsigned char SM; // Variable del estado del Sensor de seguimiento central
unsigned char SR; // Variable del estado del Sensor de seguimiento derecho
unsigned char old_SL, old_SM, old_SR; // Variables registro estado anterior de sensores seguimiento
//  Módulo de ultrasonido
int inputPin = A0; // Pin(A0) entrada ECHO módulo de ultrasonido
int outputPin = A1; // Pin(A1) entrada TRIG módulo de ultrasonido
// Variables Bluetooth
unsigned char Bluetooth_val; // Asignar variable para entrada Bluetooth - val
// Motores
#define Lpwm_pin 5              // Definir Pin(5)pwm como ENA placa accionamiento motor "control de velocidad"
#define Rpwm_pin 10             // Definir Pin(10)pwm como ENB placa accionamiento motor "control de velocidad"
int pinLB = 2;                  // Pin(2) como IN1 placa accionamiento motor
int pinLF = 4;                  // Pin(4) como IN2 placa accionamiento motor
int pinRB = 7;                  // Pin(7) como IN3 placa accionamiento motor
int pinRF = 8;                  // Pin(8) como IN4 placa accionamiento motor
unsigned char Lpwm_val = 150;   // Inicialización velocidad de la rueda izquierda en 150
unsigned char Rpwm_val = 150;   // Inicialización velocidad de la rueda derecha en 150
int Car_state = 0;              // Definir variable del estado de ejecución del coche
int servopin = 3;               // Definir Pin(3)como señal digital para comandar al servo
int myangle;                    // Definir variable ángulo
int pulsewidth;                 // Definir variable  Ancho de pulso
unsigned char ReqAng = 90;     // Inicialezar Ángulo de dirección a 90°, es decir, hacia delante
//
/////////////////////
//* 3 - FUNCIONES *//                                                                                                                                           .
/////////////////////
//* CONFIGURAR SENSORES DIRECCION
void Sensor_IO_Config()       // Función de inicialización IO para los tres módulos de seguimiento(Se inicializan como entrada)
{
  pinMode(SensorLeft, INPUT);
  pinMode(SensorMiddle, INPUT);
  pinMode(SensorRight, INPUT);
}
//* LEER  SENSORES DIRECCION
void Sensor_Scan(void)        // Función lectura de la señal de rastreo de los tres módulos
{
  SL = digitalRead(SensorLeft);
  SM = digitalRead(SensorMiddle);
  SR = digitalRead(SensorRight);
}
//* MODO SEGUIMIENTO LINEA
void FoLine_Mode(void)       // Función de modo de seguimiento del coche
{
  unsigned long IR_ReadData;
  //*lcd.print("Xun Ji ");  // Mostrar modo seguimiento
  Serial.print("\n Mostrar modo seguimiento");
  Show_V(Lpwm_val);         // Mostrar velocidad
 while ((Bluetooth_val != 'k') && (IR_ReadData != IR_Stop))
  // Mientras Bluetooth no envie 'k'  y no se pulse tecla 'NEXT' en control IR se continua en Modo Seguimiento".
    {
    if (Serial.available())Bluetooth_val = Serial.read();
    if (irrecv.decode(&results)) IR_ReadData = results.value;
    irrecv.resume();
    Serial.print("\n Mode seguimiento linea");
    Serial.print("\n IR_SL = :");       //  Ver comando IR
    Serial.print(IR_ReadData );       //  Mostar dato IR
    
    Sensor_Scan();

    if ((SL == 1 && SM == 1 && SR == 1) || (SL == 1 && SM == 1 && SR == 0) || (SL == 0 && SM == 1 && SR == 1))
      {
        advance();
        delay(150); //  Demora 150ms
      }
    if (SL == 1 && SM == 0 && SR == 0)
      {
        turnL();
        delay(150); // Demora 150ms
      }
        
    if (SL == 0 && SM == 0 && SR == 1)
      {
        turnR();
        delay(150); // Demora 150ms
      }
    
    if (SL == 0 && SM == 0 && SR == 0)
    {
      if ((old_SL == 1 && old_SM == 0 && old_SR == 0) || (old_SL == 1 && old_SM == 1 && old_SR == 0))
      {
        while (!(SL == 1 && SM == 1 && SR == 0))
        {
          Sensor_Scan();
          turnL();
          delay(150); // Demora 150ms
        }
      }
      else if ((old_SL == 0 && old_SM == 0 && old_SR == 1) || (old_SL == 0 && old_SM == 1 && old_SR == 1))
      {
        while (!(SL == 0 && SM == 1 && SR == 1))
        {
          Sensor_Scan();
          turnR();
        }
      }
    }
    //N
    if (old_SL != SL || old_SM != SM || old_SR != SR)
    {
      old_SL = SL;
      old_SM = SM;
      old_SR = SR;
    }
  }
  Serial.print("\n Esperando Señal");
  stopp();
}
//* CONVERTIR A PULSO ANGULO Y POSICIONAR SERVO  
void servopulse(int servopin, int myangle) // Definir una función de pulso
{
  pulsewidth = (myangle * 11) + 500;  // El valor del ángulo se convierte a un ancho de pulso de 500 a 2.480
  digitalWrite(servopin, HIGH);       // Los canales de servo a alto nivel
  delayMicroseconds(pulsewidth);      // Retardo microsegundos segun ancho de pulso
  digitalWrite(servopin, LOW);        // Poner a nivel bajo servopin.
  delay(20 - pulsewidth / 1000);        // Demora de 1000ms (1s)
}
//* PREPARAR SERVO Y PASAR ANGULO  A  CONTROL  PULSO
void Set_servopulse(int set_val)
{
  for (int i = 0; i <= 10; i++)     // Dar tiempo suficiente al servo.
    servopulse(servopin, set_val);  // Ir a función de impulsos de referencia
}
//* CONFIGURAR IO MOTORES
void M_Control_IO_config(void)
{
  pinMode(pinLB, OUTPUT);     // Configurar modo pinLB-> 2
  pinMode(pinLF, OUTPUT);     // Configurar modo pinLB-> 4
  pinMode(pinRB, OUTPUT);     // Configurar modo pinLB-> 7
  pinMode(pinRF, OUTPUT);     // Configurar modo pinLB-> 8
  pinMode(Lpwm_pin, OUTPUT);  // Configurar modo Lpwm_pin(PWM)->5 (ENA)
  pinMode(Rpwm_pin, OUTPUT);  // Configurar modo Lpwm_pin(PWM)->10 (ENB)
}
//* AJUSTAR VELOCIDAD
void Set_Speed(unsigned char Left, unsigned char Right) // Ajuste de la velocidad del vehículo,
{
  analogWrite(Lpwm_pin, Left);
  analogWrite(Rpwm_pin, Right);
}
//* AVANCE
void advance()                 // Marcha hacia adelante
{
  digitalWrite(pinRB, LOW);   // Motor derecho encendido normal.
  digitalWrite(pinRF, HIGH);
  digitalWrite(pinLB, LOW);   // Motor izquierdo encendido normal.
  digitalWrite(pinLF, HIGH);
  Car_state = 1;
  show_state();  // Mostar estado
}
//* GIRO DERECHA
void turnR()                  // Girar a la derecha
{
  digitalWrite(pinRB, LOW);   // Motor derecho encendido normal.
  digitalWrite(pinRF, HIGH);
  digitalWrite(pinLB, HIGH);  // Motor izquierdo encendido inverso.
  digitalWrite(pinLF, LOW); 
  Car_state = 4;
  show_state();
}
//* GIRO IZQUIERDA
void turnL()                  // Girar a la izquierda
{
  digitalWrite(pinRB, HIGH);  // Motor derecho encendido inverso.
  digitalWrite(pinRF, LOW); 
  digitalWrite(pinLB, LOW);   // Motor izquierdo encendido normal.
  digitalWrite(pinLF, HIGH);
  Car_state = 3;
  show_state();
}
//* PARAR MOTORES
void stopp()                  // Detener
{
  digitalWrite(pinRB, HIGH);  // Motor derecho apagado.
  digitalWrite(pinRF, HIGH);
  digitalWrite(pinLB, HIGH);  // Motor izquierdo apagado.
  digitalWrite(pinLF, HIGH);
  Car_state = 5;
  show_state();
}
//* REVERSA
void back()                   // Marcha para atrás
{
  digitalWrite(pinRB, HIGH);  // Motor derecho encendido inverso.
  digitalWrite(pinRF, LOW);
  digitalWrite(pinLB, HIGH);  // Motor izquierdo encendido inverso.
  digitalWrite(pinLF, LOW);
  Car_state = 2;
  show_state() ;
}
//* MOSTRAR ESTADO
void show_state(void)
{
  switch (Car_state)
  {
    case 1: Serial.print(" \r\n GO");
      break;
    case 2: Serial.print(" \r\n Back");
      break;
    case 3: Serial.print(" \r\n Left");
      break;
    case 4: Serial.print(" \r\n Right");
      break;
    case 5: Serial.print(" \r\n Stop");
      break;
    default:
      break;
  }
}
//* MOSTRAR VELOCIDAD
void Show_V(unsigned char V)
{
  Serial.print("\n Speed = ");
  Serial.print(V, DEC);
}
//* CONTROL INFRAROJO
void Show_ReqAng(unsigned char Angle)
{
  Serial.print("\n Angulo = ");
  Serial.print(Angle, DEC);
}
//* CONTROL BLUETOOTH
void Bluetooth_Control()      // Modo de control de Bluetooth,
{
  Show_V(Lpwm_val);
  Serial.print("\n Modo control Bluetooth");
  while ((Bluetooth_val != 'j')&&(Bluetooth_val != 'h')&&(Bluetooth_val != 'g'))
  { 
    if (Serial.available() > 0)   // Determinar si se recibieron datos puerto serial
    {
      Bluetooth_val = Serial.read(); // Leer datos serial y colocar en variable BT -> val;
      //Nota: Mostrar datos
      Serial.print("\n BT = :");
      Serial.print(Bluetooth_val);
      //Nota: Detrerminar acciones
      switch (Bluetooth_val)
      {
        case 'p': if (ReqAng <= 180) { // Girar a la izquierda modulo ultrasónico
            ReqAng += 10;
            Set_servopulse(ReqAng);
            Show_ReqAng(ReqAng);
          }
          break;
        case 'q': if (ReqAng - 10 >= 0) { // Girar a la derecha modulo ultrasónico
            ReqAng -= 10;
            Set_servopulse(ReqAng);
            Show_ReqAng(ReqAng);
          }
          break;
        case 'a': advance(); // Avanzar
          break;
        case 'e': back();   // Retroceder
          break;
        case 'b': turnL();  // Girar a la izquierda
          break;
        case 'd': turnR();  // Girar a la derecha
          break;
        case 'c': stopp();  // Parar
          break;
        case 'n': if (Rpwm_val + 10 <= 250 && Rpwm_val + 10 <= 250) { // Aumentar velocidad 'n'
            Lpwm_val += 10; Rpwm_val += 10;
            Set_Speed(Lpwm_val, Rpwm_val);
            Show_V(Lpwm_val);
          }
          break;
        case 'o': if (Rpwm_val - 10 >= 0 && Rpwm_val - 10 >= 0) { // Reducir velocidad 'o'
            Lpwm_val -= 10; Rpwm_val -= 10;
            Set_Speed(Lpwm_val, Rpwm_val);
            Show_V(Lpwm_val);
          }
          break;
        case 'k': Ultrasonic_Ranging(1);                  //   
          break;
        default: stopp();
          break;
      }
    }
  }
  Serial.print("\n En espera - FIN Control Bluetooth");
  stopp();
}
//* CONTROL INFRAROJO
void IR_Control(void) // Modo de control remoto, presionar "9" para salir de este modo
{
  unsigned long Key;
  Show_V(Lpwm_val); // Muestra velocidad
  Show_ReqAng(ReqAng);// Muestra el ángulo actual del pin actual servo US
  Serial.print("\n Modo control remoto IR");
  while (Key != IR_ESC) // Mientras código IR distinto a "9"
  {
    if (irrecv.decode(&results)) // Determinar si el indicador de datos en serie recibida.
    {
      Key = results.value; //  Guardar en “Key” La clave que se leyó.
      switch (Key)
      {
        case IR_Servo_L: if (ReqAng <= 180) { // Girar a la izquierda servo
            ReqAng += 10;
            Set_servopulse(ReqAng);
            Show_ReqAng(ReqAng);
          }
          break;
        case IR_Servo_R: if (ReqAng - 10 >= 0) { //  Girar a la derecha servo
            ReqAng -= 10;
            Set_servopulse(ReqAng);
            Show_ReqAng(ReqAng);
          }
          break;
        case IR_Speed_UP: if (Rpwm_val + 10 <= 250 && Rpwm_val + 10 <= 250) { //Aumentar velocidad Motores
            Lpwm_val += 10; Rpwm_val += 10;
            Set_Speed(Lpwm_val, Rpwm_val);
            Show_V(Lpwm_val);
          }
          break;
        case IR_Speed_DOWN: if (Rpwm_val - 10 >= 0 && Rpwm_val - 10 >= 0) { // Reducir velocidad Motores
            Lpwm_val -= 10; Rpwm_val -= 10;
            Set_Speed(Lpwm_val, Rpwm_val);
            Show_V(Lpwm_val);
          }
          break;
        case IR_Utr_Ran: Ultrasonic_Ranging(1); //  Modifica alcance ultrasónico
          break;
        case IR_FoLine_Mode: FoLine_Mode();   // Modo seguimiento linea "2"
          break;
        case IR_AvObj_Mode: Self_Control();   // Modo auto control ultrasónico "8"
          break;
        case IR_Go: advance();  // Adelante "CH"
          break;
        case IR_Back: back();   // Atras "Vol+"
          break;
        case IR_Left: turnL();  // Izquierda "PREV"
          break;
        case IR_Right: turnR(); // Derecha  "PLAY PAUSE"
          break;
        case IR_Stop: stopp();  // Stop   "NEXT"
          break;
        default:
          break;
      }
      irrecv.resume();        // Borra clave IR
    }
  }
  Serial.print("\n En espera - FIN Control IR");
  stopp();
}
//* EVITAR OBSTACULOS
void Self_Control(void)       // Autopropulsado, y evitar obstáculos ultrasónico
{
  int H;
  unsigned long IR_ReadData;  // Leer comando IR
  Show_V(Lpwm_val); // Mostar velocidad
  Serial.print("\n Modo control Evitar obstaculos");
 while ((Bluetooth_val != 'l')&&(IR_ReadData != (IR_Stop)))
  // Mientras bluetooth distinto "l" y IR_Stop distinto "NEXT continua modo "Evitar obstaculos"
  {
    if (Serial.available() > 0)       // Determinar si datos en serie recibidos.
    Bluetooth_val = Serial.read();    // Dato serial a variable;
    if (irrecv.decode(&results)) IR_ReadData = results.value;  // Leer IR
    irrecv.resume();                                           // Limpiar buffer IR
    Serial.print("\n BT EO = :");     // 
    Serial.print(Bluetooth_val);      // Mostar dato BT
    Serial.print("\n IR EO = :");     // 
    Serial.print(IR_ReadData );       // Mostar dato IR
    H = Ultrasonic_Ranging(1);        //
    if (H < 25)
     {
      stopp();
      while ((Ultrasonic_Ranging(1) < 50)&& (IR_ReadData !=IR_Stop))   
      {
        if ((Ultrasonic_Ranging(1) < 15)&& (IR_ReadData !=IR_Stop))
        {
          back();
          delay(500);
        }
        else
        {
          turnL();
          delay(1000);
        }
      }
    } 
       if ((H > 25)&& (IR_ReadData !=IR_Stop))
          {
          advance();
              if (irrecv.decode(&results)) IR_ReadData = results.value;  //  Leer entrada IR
              irrecv.resume();  // Borrar buffer IR
              delay(150);       // Demora 150ms 
          }
        //  
  }
  Serial.print("\n En espera - FIN Control EO");
  stopp();
}
//* MEDIR DISTANCIA
int Ultrasonic_Ranging(unsigned char Mode)// Función Alcance ultrasónica. 
    // NOTA: MODE = 1, muestra en pantalla diferente no muestra.
{
  int old_distance;
  digitalWrite(outputPin, LOW);     // Apaga PIN(A1)TRIG en ultrasónica
  delayMicroseconds(2);             // Demora de 2 micro segundos
  digitalWrite(outputPin, HIGH);    // Enciende PIN(A1)TRIG
  delayMicroseconds(10);            // Durante 10 micro segundos lo mantiene en Alto
  digitalWrite(outputPin, LOW);     // Vuelve a apagar PIN(A1)TRIG
  int distance = pulseIn(inputPin, HIGH); // Leer duracion en alto PIN(A0) ECHO
  distance = distance / 58;         // Transform pulse time to distance
  if (Mode == 1) {
    Serial.print("\n H = ");        // Mode=1 muestra distancia
    Serial.print(distance, DEC);    //
    return distance;                // Devuelve distancia
  }
  else return distance;
}
////////////////
//* 4 - SETUP //                                                                                                                                                        .
////////////////
void setup()
{
  pinMode(servopin, OUTPUT);      // Configuración de una interfaz de servo interfaz de salida
  M_Control_IO_config();          // Módulo de control del motor inicializa IO
  Set_Speed(Lpwm_val, Rpwm_val);  // El ajuste inicial de velocidad
  Set_servopulse(ReqAng);        // Ángulo de dirección de ajuste inicial
  Sensor_IO_Config();             // IO módulo de seguimiento de inicialización
  irrecv.enableIRIn();            // Se inicia la recepción de una señal de control remoto por infrarrojos
  pinMode(inputPin, INPUT);       //  Módulo de ultrasonidos IO
  pinMode(outputPin, OUTPUT);     // Módulo ultrasónico ultrasónico Módulo IO
  Serial.begin(9600);             // Inicializar el puerto serie, para Bluetooth. Velocidad 9600 baudios.
  Serial.print("\n Esperando Señal");
  stopp();                        // Parar motores
  delay(100);                     // Estabilizar
}
//////////////////////////
/// * 5 - MAIN PROGRAM ///                                                                                                                                .  
//////////////////////////
void loop()
{
  if (Serial.available() > 0)   // Determina si la bandera de datos en serie recibida.
  {
    Bluetooth_val = Serial.read(); //
    //Nota: Mostrar datos
    Serial.print("\n BT1 = :");
    Serial.print(Bluetooth_val);
    if (Bluetooth_val == 'f') Bluetooth_Control(); // Si por Bluetooth se recibe 'f' -> modo: "Control Bluetooth"
    if (Bluetooth_val == 'h') Self_Control();   // Si por Bluetooth se recibe 'h' -> modo: "Evasión de obstáculos"
    if (Bluetooth_val == 'g') FoLine_Mode();     // Si por Bluetooth se recibe 'g' -> modo: "Seguimiento linea"
  }
  if (irrecv.decode(&results)) {                // Tras la recepción de una clave por el control remoto IR
    if (results.value == IR_Stop )IR_Control(); // Si en control remoto se presiona tecla "OK", entra en modo: "Control remoto IR"
    irrecv.resume();                            // Borrar la clave infrarroja actual, la clave es leer la siguiente lista
  }
  delay(10);
}
///////////
/// END ///                                                                                                                                                                 .
///////////