#include <Servo.h>

Servo myServo;  // Cria um objeto Servo

const int trigPin = 8;  // Pino Trig do sensor ultrassônico
const int echoPin = 9; // Pino Echo do sensor ultrassônico

int servoPin = 2;       // Pino de controle do Servo
int angle = 0;          // Ângulo atual do servo
int step = 2;           // Incremento/decremento do ângulo

void setup() {
  myServo.attach(servoPin);  // Conecta o servo ao pino
  pinMode(trigPin, OUTPUT);  // Configura Trig como saída
  pinMode(echoPin, INPUT);   // Configura Echo como entrada
  Serial.begin(9600);        // Inicia comunicação serial

  myServo.write(0);          // Posiciona o servo em 90°
  delay(500);
}

void loop() {
  // Posiciona o servo no ângulo atual
  myServo.write(angle);
  delay(50);           // Aguarda o servo se mover

  // Mede a distância
  int distance = measureDistance();

  // Envia o ângulo e a distância pela serial
  Serial.print(angle);
  Serial.print(",");
  Serial.println(distance);

  // Ajusta o ângulo
  angle += step;
  if (angle >= 180 || angle <= 0) {
    step = -step;  // Inverte a direção quando atingir os limites
  }
  delay(50);  // Pequena pausa antes do próximo movimento
}

// Função para medir a distância usando o sensor ultrassônico
int measureDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;  // Converte para cm

  // Retorna 0 se a distância for inválida
  return (distance > 0 && distance <= 400) ? distance : 0;
}