/*
   This ESP32 code is created by esp32io.com

   This ESP32 code is released in the public domain

   For more detail (instruction and wiring diagram), visit https://esp32io.com/tutorials/esp32-ultrasonic-sensor-piezo-buzzer
*/
#include<ESP32Servo.h>
#define TRIG_PIN   26 // ESP32 pin GPIO26 connected to Ultrasonic Sensor's TRIG pin
#define ECHO_PIN   25 // ESP32 pin GPIO25 connected to Ultrasonic Sensor's ECHO pin
#define BUZZER_PIN 17 // ESP32 pin GPIO17 connected to Piezo Buzzer's pin
#define DISTANCE_THRESHOLD 50 // centimeters

Servo s;
int p = 0;

float duration_us, distance_cm,emergencyalarm;

void setup() {
  Serial.begin (9600);         // initialize serial port
  pinMode(TRIG_PIN, OUTPUT);   // set ESP32 pin to output mode
  pinMode(ECHO_PIN, INPUT);    // set ESP32 pin to input mode
  pinMode(BUZZER_PIN, OUTPUT); // set ESP32 pin to output mode
  pinMode(22, INPUT);
  s.attach(18);
}

void loop() {

  if (p <= 180)
  {
    Serial.print(p);
    Serial.print(" ");
    s.write(p);
    p = p + 1;
    // generate 10-microsecond pulse to TRIG pin
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    // measure duration of pulse from ECHO pin
    duration_us = pulseIn(ECHO_PIN, HIGH);
    
    // calculate the distance
    distance_cm = 0.017 * duration_us;

    if (distance_cm < DISTANCE_THRESHOLD)
    {
      tone(BUZZER_PIN, 500, 50);
      delay(100);
    } // turn on Piezo Buzzer
    else
      digitalWrite(BUZZER_PIN, LOW);  // turn off Piezo Buzzer

    // print the value to Serial Monitor
    //Serial.print(p);

    Serial.print("distance: ");
    Serial.print(distance_cm);

    Serial.println(" cm");

    delay(10);
  }
  else
  {
  p = 0;
  }
if((digitalRead(22))==HIGH)
    {
    Serial.print("Latitude: ");
    Serial.println("40°45'11''N"); //default latitude
    Serial.print("Longitude: ");
    Serial.println("73°58'59''W"); //default longitude
    }


  /*for(p=180;p>=0;p--)
    {
    s.write(p);
    // generate 10-microsecond pulse to TRIG pin
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    // measure duration of pulse from ECHO pin
    duration_us = pulseIn(ECHO_PIN, HIGH);
    // calculate the distance
    distance_cm = 0.017 * duration_us;

    if (distance_cm < DISTANCE_THRESHOLD)
    {
    tone(BUZZER_PIN, 500,50);
    delay(100);
    }// turn on Piezo Buzzer
    else
    digitalWrite(BUZZER_PIN, LOW);  // turn off Piezo Buzzer

    // print the value to Serial Monitor
    Serial.print("distance: ");
    Serial.print(distance_cm);
    Serial.println(" cm");

    delay(10);
    }
  */

}