#include <NewPing.h>
#include <Servo.h>
#define trigPin 13
#define echoPin 12
#define MAX_DISTANCE 500

NewPing sonar(trigPin, echoPin, MAX_DISTANCE);

int pinSpeaker = 3;
int angle = 0;

Servo servo;


void setup() {
  Serial.begin(115200);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(pinSpeaker, OUTPUT);
  servo.attach(9);
  // put your setup code here, to run once:

}

void loop() {
  int duration, distance, pos = 0, i;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration / 2) / 29.1;
  Serial.print(distance);
  Serial.println("cm");
  if (distance <= 5) {
    digitalWrite(pinSpeaker, HIGH);
    playTone(300,160);
  } else if (distance <= 20) {
      digitalWrite(pinSpeaker, HIGH);
      playTone(300,160);
      delay(200);
    } else if (distance <= 50) {
      digitalWrite(pinSpeaker, HIGH);
      playTone(300,160);
      delay(500);

    }
  } 
  

  // put your main code here, to run repeatedly:

void playTone(long duration, int freq) {

  duration *= 1000;
  int period = (1.0 / freq) * 1000000;
  long elapsed_time = 0;
  while (elapsed_time < duration) {
    digitalWrite(pinSpeaker, HIGH);
    delayMicroseconds(period / 2);
    digitalWrite(pinSpeaker, LOW);
    delayMicroseconds(period / 2);
    elapsed_time += (period);
  }
}