#include <Ultrasonic.h>

Ultrasonic ultrasonic(22,23);   //Ultrasonic ultrasonic(4,3);
#define pwma 15        // declaramos los pines del puente h y las salidas de arduino 
#define ain2 2
#define ain1 4
#define stby 5
#define bin1 18
#define bin2 19
#define pwmb 21
#define IR_sensor_front A0 // front sensor
#define IR_sensor_back A1 // rear senson
int distance ;


void adelante(){
  // adelante
  analogWrite(pwma,100);          //velocidad del motor derecho.
  analogWrite(pwmb,100);          //velocidad del motor izquierdo.
  digitalWrite(stby,1);           //encendemos el standby del puente h tb6612 modo espera
  digitalWrite(ain1,1);
  digitalWrite(ain2,0);
  digitalWrite(bin1,1);
  digitalWrite(bin2,0);
  delay(800);
}

void atras() {
  // atrás
  analogWrite(pwma,100);          //velocidad del motor derecho.
  analogWrite(pwmb,100);          //velocidad del motor izquierdo.
  digitalWrite(stby,1);           //encendemos el standby del puente h tb6612 modo espera
  digitalWrite(ain1,0);
  digitalWrite(ain2,1);
  digitalWrite(bin1,0);
  digitalWrite(bin2,1);
  delay(800);
}

void derecha(){
  // derecha
  analogWrite(pwma,100);          //velocidad del motor derecho.
  analogWrite(pwmb,100);          //velocidad del motor izquierdo.
  digitalWrite(stby,1);           //encendemos el standby del puente h tb6612 modo espera
  digitalWrite(ain1,1);
  digitalWrite(ain2,0);
  digitalWrite(bin1,0);
  digitalWrite(bin2,1);
  delay(800);
}

void izquierda(){
  // izquierda
  analogWrite(pwma,100);          //velocidad del motor derecho.
  analogWrite(pwmb,100);          //velocidad del motor izquierdo.
  digitalWrite(stby,1);           //encendemos el standby del puente h tb6612 modo espera
  digitalWrite(ain1,0);
  digitalWrite(ain2,1);
  digitalWrite(bin1,1);
  digitalWrite(bin2,0);
  delay(800);
}

void paro() {
  // paro
  analogWrite(pwma,100);          //velocidad del motor derecho.
  analogWrite(pwmb,100);          //velocidad del motor izquierdo.
  digitalWrite(stby,1);           //encendemos el standby del puente h tb6612 modo espera
  digitalWrite(ain1,0);
  digitalWrite(ain2,0);
  digitalWrite(bin1,0);
  digitalWrite(bin2,0);
  delay(800);
}

#define IR_sensor_front A5 // front sensor
#define IR_sensor_back A4 // rear sensor

void setup() {
  Serial.begin(9600);
  pinMode(pwma,OUTPUT);   //definimnos como salidas todos los pines para utilizar el puente h
  pinMode(ain2,OUTPUT); 
  pinMode(ain1,OUTPUT); 
  pinMode(stby,OUTPUT); 
  pinMode(bin1,OUTPUT); 
  pinMode(bin2,OUTPUT); 
  pinMode(pwmb,OUTPUT);  
}

void loop() {
  int IR_front = analogRead(IR_sensor_front);
  int IR_back = analogRead(IR_sensor_back);
  distance = ultrasonic.read();
  izquierda(); // start rotete if (distance < 20){
  paro();
  
  while (distance < 20 ) {
    adelante(); 
    distance = ultrasonic.read();
    IR_front = analogRead(IR_sensor_front);
    IR_back = analogRead(IR_sensor_back);
    if ( IR_front > 650 || IR_back > 650 ) { break;}
    delay(10);
  }
  
  
  if (IR_front < 650 )  // < 650 means white line
  {
    paro();
    delay (50);
    atras();
    delay (500);
  } 
   
  if (IR_back < 650 )  //
   {
   paro();
   delay (50);
   adelante();
   delay (500);
   }
   

/*  adelante();
  paro();
  atras();
  paro();
  izquierda();
  paro();
  derecha();
  paro();*/
}