#include <Arduino.h>
#include <ESP32Servo.h>

#define TRIG_PIN 5
#define ECHO_PIN 18
#define SERVO_PIN 19

Servo myServo;

void init_ultrasonic() {
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
    digitalWrite(TRIG_PIN, LOW);
}

long get_distance() {
    long duration, distance;

    // Clear the TRIG_PIN
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);

    // Set the TRIG_PIN high for 10 microseconds
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    // Read the ECHO_PIN, pulseIn() returns the duration (microseconds) of the pulse
    duration = pulseIn(ECHO_PIN, HIGH);

    // Calculate the distance
    distance = duration * 0.43 / 3;

    return distance;
}

void setup() {
    Serial.begin(115200);
    init_ultrasonic();
    myServo.attach(SERVO_PIN);
    myServo.write(0);  // Initialize the servo to 0 degrees
}

void loop() {
    long distance = get_distance();
    Serial.print("Distance:100 ");
    Serial.print(distance);
    Serial.println(" cm");

    // Rotate the servo to 180 degrees and back to 0 degrees
    myServo.write(180);
    delay(500); // Wait for half a second
    myServo.write(0);
    delay(500); // Wait for half a second
}