#include <ESP32Servo.h>
//#include <WiFi.h>

#define Echo 16
#define Trigger 17
#define pServo 23
#define Sound 18

Servo servo;

float Durasi, Jarak;
int jMin= 10, jMid= 20, jMax= 350;
unsigned long mSonik, mSerial, mSound;

void setup() {
  Serial.begin (9600);
  pinMode(Trigger, OUTPUT);
  pinMode(Echo, INPUT);
  pinMode(Sound, OUTPUT);
  digitalWrite(Sound, LOW);
  servo.attach(pServo);
  servo.write(0);
}

void loop() {
  if(millis() - mSonik >= 50){
    mSonik = millis();
    BacaSensor();
  }
  if(millis() - mSerial >= 1000){
    mSerial = millis();
    Serial.println(Jarak);
  }
  if(Jarak < jMin){
    servo.write(180);
    digitalWrite(Sound, HIGH);
    digitalWrite(Sound, LOW);
  }
  else if(Jarak >= jMin && Jarak <= jMid){
    if(millis() - mSound >= 5000){
      mSound = millis();
      digitalWrite(Sound, HIGH);
      digitalWrite(Sound, LOW);
    }
  }
  else{
    servo.write(0);
    digitalWrite(Sound, LOW);
  }
}

void BacaSensor(){
  digitalWrite(Trigger, LOW);
  delayMicroseconds(2);
  digitalWrite(Trigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trigger, LOW);
  Durasi = pulseIn(Echo, HIGH);
  Jarak = Durasi * 0.034 / 2;
  if(Jarak > jMax) Jarak= jMax;
}