#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>

Servo myservo;                            
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);            

const int trig = 5;
const int echo = 18;
long data;
int cm;
int pos = 13;

String x = "Open";
String y = "Close";

void setup() {
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // สั่งจอ OLED เริ่มทำงานที่ address 0x3C
  display.clearDisplay();
  display.setTextColor(WHITE, BLACK); // กำหนดข้อความสีขาว ฉากหลังสีดำ
  display.setCursor(30, 10); // กำหนดตำแหน่ง x,y ที่จะแสดงผล
  // display.print(x);
  myservo.attach(13);
}

void loop() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  data = pulseIn(echo, HIGH);
  cm = data * 0.035 / 2;
  Serial.println(cm);
  delay(1000);

  display.clearDisplay();
  display.setTextColor(WHITE, BLACK);
  display.setCursor(0, 0);

  if (cm < 100) { 
    myservo.write(0); // ควบคุมตำแหน่งหมุน 0 องศา
    display.clearDisplay();
    display.setTextColor(WHITE, BLACK);
    display.setCursor(30, 28);
    display.print("centimater = ");
    display.print(cm); 
    display.setCursor(30, 40);
    display.print("information = ");
    display.print(data); 
    display.display();
  }

  if (cm > 100) { 
    myservo.write(90); // ควบคุมตำแหน่งหมุน 90 องศา
    display.setTextColor(WHITE, BLACK);
    display.clearDisplay();
    display.setTextColor(WHITE, BLACK);
    display.setCursor(30, 28);
    display.print("centimater = ");
    display.print(cm); 
    display.setCursor(30, 40);
    display.print("information = ");
    display.print(data);
    display.display();
  }
}