#include <Servo.h>
Servo myServo;
int servoPin = 10; //(D7)

const int trig = 12;     // chân trig của HC-SR04
const int echo = 11;     // chân echo của HC-SR04

float GetDistance(){
    unsigned long duration; // biến đo thời gian
    int distanceCm;           // biến lưu khoảng cách
    
    /* Phát xung từ chân trig */
    digitalWrite(trig,0);   // tắt chân trig
    delayMicroseconds(2);
    digitalWrite(trig,1);   // phát xung từ chân trig
    delayMicroseconds(5);   // xung có độ dài 5 microSeconds
    digitalWrite(trig,0);   // tắt chân trig
    
    /* Tính toán thời gian */
    // Đo độ rộng xung HIGH ở chân echo. 
    duration = pulseIn(echo,HIGH);  
    // Tính khoảng cách đến vật.
    distanceCm = int(duration/2/29.412);
    delay(200);
  
  return distanceCm;
}

void setup() {
  Serial.begin(9600);
  myServo.attach(servoPin); 
  myServo.write(0);
  pinMode(trig,OUTPUT);   // chân trig sẽ phát tín hiệu
  pinMode(echo,INPUT);    // chân echo sẽ nhận tín hiệu
}

void loop() {
  long distance = GetDistance();
 
  if (distance <= 0)
  {
    Serial.println("Quá thời gian đo khoảng cách !!");
  }
  else
  {   
    Serial.print("Khoảng cách (cm): ");
    Serial.println(distance);
  }
 
  if (distance < 20) {
     runServo();
  }
}

void runServo() {
  myServo.write(90);  // Cho servo quay một góc 90 độ
  delay(3000);
  myServo.write(0); // Cho servo trở về vị trí ban đầu
  delay(1000);
}