#include <ESP32Servo.h>

#define poten 35
#define servo 26 
#define servo1 27 

Servo myServo;  
Servo myServo1;  

const int trig = 0;
const int echo = 4;

long duration;
int distanceCm;


void setup() {
  pinMode(trig, OUTPUT); 
  pinMode(echo, INPUT); 
  Serial.begin(9600);

  myServo.attach(servo);  
  myServo1.attach(servo1);  
}

void loop() {
 
 /* int analogValue = analogRead(poten);


  int angle = map(analogValue, 0, 4095, 0, 180);
  int angle1 = map(analogValue, 0, 4095, 180, 0); */

 
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
 
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  duration = pulseIn(echo, HIGH);
  
  distanceCm = duration * 0.034/2;

Serial.println(distanceCm);


  if (distanceCm < 200) {
    myServo.write(180);
    myServo1.write(0);
  } 
  else {
     myServo.write(90);
  myServo1.write(90);
  }

  // อาจจะเพิ่มดีเลย์เพื่อไม่ให้ลูปทำงานเร็วเกินไป
  delay(100);

  /*myServo.write(angle);
  myServo1.write(angle1);

  Serial.print("มุม: ");
  Serial.println(angle);
  delay(100);*/
}