#include <Arduino.h>
#include <ESP32Servo.h>

#define TRIG_PIN 5
#define ECHO_PIN 17
#define SERVO_PIN 4
bool Horario = true;
int agPlus = 4;

Servo myServo;

void setup() {
  Serial.begin(115200); // Inicia a comunicação serial com o baud rate de 115200
  myServo.attach(SERVO_PIN); // Anexa o servo motor ao pino definido

  pinMode(TRIG_PIN, OUTPUT); // Define o pino do Trigger como saída
  pinMode(ECHO_PIN, INPUT);  // Define o pino do Echo como entrada

  myServo.write(0); // Inicia o servo motor na posição 0 graus
  delay(2000); // Espera 2 segundos para estabilizar
}

void loop() {
  if (Horario) {
    for(int angle = 0; angle <= 180; angle += agPlus) { // Move o servo de 0 a 180 graus
      LeituraSonar(angle);
    }
    Horario = false;
  }
  else {
    for(int angle = 180; angle > 0; angle -= agPlus) { // Move o servo de 0 a 180 graus
      LeituraSonar(angle);
    }
    Horario = true;
  }
  
}

void LeituraSonar(int angle) {
  myServo.write(angle); // Move o servo para o ângulo desejado
  delay(50); // Espera meio segundo para o servo se mover

  long duration, distance;
  digitalWrite(TRIG_PIN, LOW);  
  delayMicroseconds(2); 
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10); 
  digitalWrite(TRIG_PIN, LOW);
    
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = (duration / 2) / 29.1; // Calcula a distância em centímetros

  String protocolo = (String)angle + "," + (String)distance;

  Serial.println(protocolo); // Envia a distância medida
  Serial.flush();

  //delay(200); // Espera um segundo antes da próxima medição
}