//Trabajo Practico N7 Trayectoria forma de L
// Integrantes Fabiancic , Fragapane, Gargiulo , Gattas, Torres
int Xi=0;       //Definimos la posición inicial
int Yi=0;
int Xf=3000;      //Y la posición final
int Yf=3000;
int escala=31;  //Es un valor de conversión de milisegundos a cm
                //que depende del robot y fue determinado mediante pruebas con el mismo


//Pines para el control de los motores
int in1 = 9;
int in2 = 8;
int in3 = 7;
int in4 = 10;
int ENA = 6;
int ENB = 5;
int vel_ENA = 200;
int vel_ENB = 200;
//Pines para el control del sensor de ultrasonido
int Echo = A4;  
int Trig = A5; 


//Valores para controlar el PWM
int VEL = 200;        //Usado para ir hacia adelante y atrás
int GIRO = 200;       //Para girar

//Tiempos para el delay de funcionamiento de los motores
//Determinados experimentalmente con el robot
int MediaVuelta = 700;
int GiroDer = 350;
int GiroIzq = 380;

int distance;


void _mForward()
{
  analogWrite(ENA,vel_ENA);
  analogWrite(ENB,vel_ENB);
  digitalWrite(in1,LOW);
  digitalWrite(in2,HIGH);
  digitalWrite(in3,HIGH);
  digitalWrite(in4,LOW);
  //Serial.println("go forward!");
}

void _mBack()
{
  analogWrite(ENA,vel_ENA);
  analogWrite(ENB,vel_ENB);
  digitalWrite(in1,HIGH);
  digitalWrite(in2,LOW);
  digitalWrite(in3,LOW);
  digitalWrite(in4,HIGH);
  //Serial.println("go back!");
}

void _mleft()
{
  analogWrite(ENA,GIRO);
  analogWrite(ENB,GIRO);
   digitalWrite(in1,HIGH);
   digitalWrite(in2,LOW);
   digitalWrite(in3,HIGH);
   digitalWrite(in4,LOW);
   //Serial.println("go left!");
}

void _mright()
{
   analogWrite(ENA,GIRO);
  analogWrite(ENB,GIRO);
   digitalWrite(in1,LOW);
   digitalWrite(in2,HIGH);
   digitalWrite(in3,LOW);
   digitalWrite(in4,HIGH);
   //Serial.println("go right!");
} 
void _mStop()
{
   digitalWrite(ENA,LOW);
   digitalWrite(ENB,LOW);
   //Serial.println("Stop!");
} 

//El robot no inicia su rutina hasta que no detecte algo a menos de 5 cm
void iniciar()
{
  while(1){
    digitalWrite(Trig, LOW);    //Se coloca en bajo para evitar perturbaciones
    delayMicroseconds(2);
    digitalWrite(Trig, HIGH);   //Se emite el pulso
    delayMicroseconds(20);
    digitalWrite(Trig, LOW);   
    float dist = pulseIn(Echo, HIGH);  //Mide el tiempo del pulso recibido en uS
    distance= dist/57;                //Factor de conversion a cm
  //  Serial.print(distance);
  //  Serial.println(" cm");
    if (distance<5)
      break;
  }
}
// leer ultrasonido

void leer_dist(){
  digitalWrite(Trig, LOW);    //Se coloca en bajo para evitar perturbaciones
    delayMicroseconds(2);
    digitalWrite(Trig, HIGH);   //Se emite el pulso
    delayMicroseconds(20);
    digitalWrite(Trig, LOW);   
    float dist = pulseIn(Echo, HIGH);  //Mide el tiempo del pulso recibido en uS
    distance= dist/57;  
}    


void setup() {
  Serial.begin(9600);
  pinMode(in1,OUTPUT);      
  pinMode(in2,OUTPUT);
  pinMode(in3,OUTPUT);
  pinMode(in4,OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
  pinMode(Echo, INPUT);     //Pin para recibir la señal 
  pinMode(Trig, OUTPUT);    //Pin para emitir el pulso
  Serial.println("empieza");
}

void loop() {
  int diff=0;
  int distancia=0;
  iniciar();
  delay(1000);


//-------------------Hace una trayectoria y si encuentra un obstaculo se frena---------------------------------------
  while (1){
    leer_dist();
    
    
    if (Yi<Yf){
      diff=Yf-Yi;
      distancia = diff*escala;
      _mForward();
      delay(1);
      for( int i =0 ; i< distancia ; i++){
        _mForward();
        leer_dist();
        Serial.println(distance);
        if(  distance < 15 ){
            _mStop();
            Serial.println("entro");
            break;
        }
        delay(1);
      }
      Yi=Yf;
    }
    else if (Yi>Yf){
      diff=Yi-Yf;
      distancia = diff*escala;
      _mright();
      delay(MediaVuelta);
      _mForward();
      delay(distancia);
      Yi=Yi-1;;
    }
    else if (Xi<Xf){
      diff=Xf-Xi;
      distancia = diff*escala;
      _mright();
      delay(GiroDer);
      for( int i =0 ; i< distancia ; i++){
        _mForward();
        leer_dist();
        Serial.println(distance);
       if(  distance < 15 ){
          _mStop();
          break;
        }
        delay(1);
      }
      Xi=Xf;
    }
    else if (Xi>Xf){
      diff=Xi-Xf;
      distancia = diff*escala;
      _mleft();
      delay(GiroIzq);
      for( int i =0 ; i< distancia ; i++){
        _mForward();
        leer_dist();
        //Serial.println(distance);
       if(  distance < 15 ){
          _mStop();
          break;
        }
        delay(1);
      }
      
      Xi=Xf;
    }
    else{
      _mStop();
      break;
    }
  }
}
