#include <Servo.h>
const int TRIG_PIN = 2;
const int ECHO_PIN = 3;
const int LED_PIN = 4;
const int BUZZER_PIN = 5;
const int SERVO_PIN = 9;
Servo radarServo;
long medirDistancia() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duracao = pulseIn(ECHO_PIN, HIGH);
  long distancia = duracao * 0.034 / 2;
  return distancia;
}
void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  radarServo.attach(SERVO_PIN);
  Serial.begin(115200);
}
void loop() {
  for (int angulo = 0; angulo <= 180; angulo++) {
    radarServo.write(angulo);
    long distancia = medirDistancia();
    Serial.print("Angulo: ");
    Serial.print(angulo);
    Serial.print(" | Distancia: ");
    Serial.print(distancia);
    Serial.println(" cm");
    controlarAlarme(distancia);
    delay(20);
  }
  for (int angulo = 180; angulo >= 0; angulo--) {
    radarServo.write(angulo);
    long distancia = medirDistancia();
    Serial.print("Angulo: ");
    Serial.print(angulo);
    Serial.print(" | Distancia: ");
    Serial.print(distancia);
    Serial.println(" cm");
    controlarAlarme(distancia);
    delay(20);
  }
}
void controlarAlarme(long distancia) {
  if (distancia > 0 && distancia < 50) {
    digitalWrite(LED_PIN, HIGH);
    int frequencia = map(distancia, 50, 0, 500, 3000);
    tone(BUZZER_PIN, frequencia);
  } else {
    digitalWrite(LED_PIN, LOW);
    noTone(BUZZER_PIN);
  }
}