/*------------------------------------------ */
/* programa seguidor de línea robot arduino  */
/*------------------------------------------ */

#include <LEANTEC_ControlMotor.h> //Incluimos la libreria para el control de motores.

// Conecta los sensores
#define LEFT_SENSORPIN   11
#define RIGHT_SENSORPIN  10

// Configuramos los pines que vamos a usar de salida
ControlMotor control(2,3,7,4,5,6);
int MotorDer1=2;//El pin 2 de arduino se conecta con el pin In1 del L298N
int MotorDer2=3;//El pin 3 de arduino se conecta con el pin In2 del L298N
int MotorIzq1=7;//El pin 7 de arduino se conecta con el pin In3 del L298N
int MotorIzq2=4;//El pin 4 de arduino se conecta con el pin In4 del L298N
int PWM_Derecho=5;//El pin 5 de arduino se conecta con el pin EnA del L298N
int PWM_Izquierdo=6;//El pin 6 de arduino se conecta con el pin EnB del L298N

int velocidad=150;//Declaramos una variable para guardar la velocidad


// Definimos variables de los sensores de entrada
int sensor_R=0; // Variable para guardar el valor del sensor de la derecha
int sensor_L=0; // Variable para guardar el valor del sensor de la izquierda


void setup()
{

  // Iniciamos el puerto serie para volcar datos al PC
  Serial.begin(9600);

  //Configuramos los pines como salida
  pinMode(MotorDer1, OUTPUT);
  pinMode(MotorDer2, OUTPUT);
  pinMode(MotorIzq1, OUTPUT);
  pinMode(MotorIzq2, OUTPUT);
  pinMode(PWM_Derecho, OUTPUT);
  pinMode(PWM_Izquierdo, OUTPUT);
}


void derecha_antihorario_izquierda_horario(){
/*En esta fución la rueda derecha girará en sentido antihorario y la
izquierda en sentido horario.
En este caso, si las ruedas estuvieran en el chasis de un robot, el
robot retrocederia.*/
  digitalWrite(MotorDer1,HIGH);
  digitalWrite(MotorDer2,LOW);
  digitalWrite(MotorIzq1,HIGH);
  digitalWrite(MotorIzq2,LOW);
  analogWrite(PWM_Derecho,200);//Velocidad motor derecho 200
  analogWrite(PWM_Izquierdo,200);//Velocidad motor izquierdo 200

}
void derecha_horario_izquierda_antihorario(){
/*En esta fución la rueda derecha girará en sentido horario y la
izquierda en sentido antihorario.
En este caso, si las ruedas estuvieran en el chasis de un robot, el
robot avanzaría.*/
  digitalWrite(MotorDer1,LOW);
  digitalWrite(MotorDer2,HIGH);
  digitalWrite(MotorIzq1,LOW);
  digitalWrite(MotorIzq2,HIGH);
  analogWrite(PWM_Derecho,200);//Velocidad motor derecho 200
  analogWrite(PWM_Izquierdo,200);//Velocidad motor izquierdo 200

}
void giro_horario(){
/*En esta fución ambas ruedas girarán en sentido horario.
En este caso, si las ruedas estuvieran en el chasis de un robot, el
robot giraria a la derecha.*/
  digitalWrite(MotorDer1,HIGH);
  digitalWrite(MotorDer2,LOW);
  digitalWrite(MotorIzq1,LOW);
  digitalWrite(MotorIzq2,HIGH);
  analogWrite(PWM_Derecho,200);//Velocidad motor derecho 200
  analogWrite(PWM_Izquierdo,200);//Velocidad motor izquierdo 200

}
void giro_antihorario(){
/*En esta fución ambas ruedas girarán en sentido antihorario.
En este caso, si las ruedas estuvieran en el chasis de un robot, el
robot giraria a la izquierda.*/
  digitalWrite(MotorDer1,LOW);
  digitalWrite(MotorDer2,HIGH);
  digitalWrite(MotorIzq1,HIGH);
  digitalWrite(MotorIzq2,LOW);
  analogWrite(PWM_Derecho,200);//Velocidad motor derecho 200
  analogWrite(PWM_Izquierdo,200);//Velocidad motor izquierdo 200

}
void parar(){
/*Función para que las ruedas paren*/
  digitalWrite(MotorDer1,LOW);
  digitalWrite(MotorDer2,LOW);
  digitalWrite(MotorIzq1,LOW);
  digitalWrite(MotorIzq2,LOW);
  analogWrite(PWM_Derecho,200);//Velocidad motor derecho 200
  analogWrite(PWM_Izquierdo,200);//Velocidad motor izquierdo 200

}
void loop()
{
  // Leemos sensores L y H
  sensor_L=digitalRead(LEFT_SENSORPIN);
  sensor_R=digitalRead(RIGHT_SENSORPIN);

  // Visualizamos
  Serial.print(" Left : ");
  Serial.print(sensor_L);
  Serial.print(" Right : ");
  Serial.print(sensor_R);
  Serial.println();

  // En función del estado de los sensores seguimos recto, giramos o paramos
  // Estado: High->Negro , Low->blanco
  if ((sensor_L==LOW)&&(sensor_R==LOW)) {
    // Sigue recto
    Serial.println("Recto");
    giro_horario(); //Llamamos a la función giro_horario
    delay(10);
  }
  else if ((sensor_L==HIGH)&&(sensor_R==LOW)) {
    // Sensor izquierdo detecta negro -> Gira izquierda
    Serial.println("Giro izquierda");
    derecha_horario_izquierda_antihorario();  //Llamamos a la función derecha_horario_izquierda_antihorario
    delay(10);
  }
  else if ((sensor_L==LOW)&&(sensor_R==HIGH)) {
    // Sensor derecho detecta negro -> Gira derecha
    Serial.println("Giro derecha");
    derecha_antihorario_izquierda_horario(); //Llamamos a la función derecha_antihorario_izquierda_horario
    delay(10);
  }
  else {
    // los dos sensores leen negro. se para
    Serial.println("Paro");
    parar();//Llamamos a la función parar
    delay(10);
  }


}