//Inclui a biblioteca do sensor HC SR04c
#include <HCSR04.h>
//Define os pinos do Arduino D8 e D9 como porta do ECHO e TRIGGER  
#define TRIGGER   4
#define ECHO      5
// Inicializa o sensor usando os pinos TRIGGER and ECHO.
UltraSonicDistanceSensor distanceSensor(TRIGGER, ECHO);  

#include <Servo.h> //INCLUSÃO DA BIBLIOTECA NECESSÁRIA
 const int pinoServo = 6; //PINO DIGITAL UTILIZADO PELO SERVO  
 Servo s; //OBJETO DO TIPO SERVO
 int pos; //POSIÇÃO DO SERVO
 int ang_maior_dist; // ângulo da maior distância
 int maior_dist_obst;
 int distancia;
 int dist_obst;


void setup () {
  //Inicializa a porta serial do Arduino com 9600 de baud rate
    Serial.begin(9600);
   
     s.attach(pinoServo); //ASSOCIAÇÃO DO PINO DIGITAL AO OBJETO DO TIPO SERVO
     s.write(90); //INICIA O MOTOR NA POSIÇÃO 0º
     ang_maior_dist = 0;
     maior_dist_obst = 0;
}
void loop () {
  delay(1000);
  distancia = distanceSensor.measureDistanceCm();
  if(distancia>0 & distancia<50){
    maior_dist_obst = 0;
    melhor_rota_desvio();
  }
 
}
    

//DEFINE ROTA - OBSTACULO
void melhor_rota_desvio(){

 //ESQUERDA
    for(pos = 0; pos < 180; pos++){ //PARA "pos" IGUAL A 0, ENQUANTO "pos" MENOR QUE 180, INCREMENTA "pos"
      s.write(pos); //ESCREVE O VALOR DA POSIÇÃO QUE O SERVO DEVE GIRAR
      delay(15); //INTERVALO DE 15 MILISSEGUNDOS
      dist_obst = distanceSensor.measureDistanceCm();
      if (dist_obst > maior_dist_obst){
        maior_dist_obst = dist_obst;
        ang_maior_dist = pos;
      }
      
    }

    

 //DIREITA
    for(pos = 180; pos >= 90; pos--){ //PARA "pos" IGUAL A 180, ENQUANTO "pos" MAIOR OU IGUAL QUE 0, DECREMENTA "pos"
      s.write(pos); //ESCREVE O VALOR DA POSIÇÃO QUE O SERVO DEVE GIRAR
      delay(15); //INTERVALO DE 15 MILISSEGUNDOS
      dist_obst = distanceSensor.measureDistanceCm();
      if (dist_obst >= maior_dist_obst){
        maior_dist_obst = dist_obst;
        ang_maior_dist = pos;
      }
      
    }

Serial.println(maior_dist_obst);

Serial.println(ang_maior_dist);
  
 }