// นายภูดิศ สิมทอง ม.5/1 เลขที่ 13
#include <Servo.h> // include the Servo library

Servo myservo; // create a servo object

// define the pins for Ultrasonic Sensor HC-SR04
const int trigPin = 9;  // define the trigPin and trigPin for the Ultrasonic Sensor HC-SR04
const int echoPin = 10; // define the trigPin and echoPin for the Ultrasonic Sensor HC-SR04
const int trigPin2 = 11;  // define the trigPin and trigPin for the Ultrasonic Sensor HC-SR04
const int echoPin2 = 12; // define the trigPin and echoPin for the Ultrasonic Sensor HC-SR04
const int full =100;
const int LED1 =7 ;
const int LED2 = 6;


void setup() {
  myservo.attach(8); // attach the servo to pin 8
  pinMode(trigPin, OUTPUT); // set the trigPin as an output
  pinMode(echoPin, INPUT); // set the echoPin as an input
  pinMode(trigPin2, OUTPUT); // set the trigPin as an output
  pinMode(echoPin2, INPUT); // set the echoPin as an input
 
  // start the serial communication
  Serial.begin(9600);

}

void loop() {
  long duration, distance; // create variables to store the duration and distance
  
  digitalWrite(trigPin, LOW); // set the trigPin low
  delayMicroseconds(2); // wait for 2 microseconds
  
  digitalWrite(trigPin, HIGH); // set the trigPin high
  delayMicroseconds(10); // wait for 10 microseconds
  
  digitalWrite(trigPin, LOW); // set the trigPin low
  
  duration = pulseIn(echoPin, HIGH); // read the duration of the pulse
  distance = (duration/2) / 29.1; // calculate the distance in centimeters
  
   if (distance < 200) { // if the distance is less than 20 cm
    myservo.write(0); // set the servo to 0 degrees
    delay(1000); // wait for 1 second
    myservo.write(90); // set the servo to 90 degrees
  }
  else { // if the distance is greater than 20 cm
    myservo.write(90); // set the servo to 90 degrees
        Serial.println(distance);
  }
  ////////
  long duration2, distance2; // create variables to store the duration and distance
  
  digitalWrite(trigPin2, LOW); // set the trigPin low
  delayMicroseconds(2); // wait for 2 microseconds
  
  digitalWrite(trigPin2, HIGH); // set the trigPin high
  delayMicroseconds(10); // wait for 10 microseconds
  
  digitalWrite(trigPin2, LOW); // set the trigPin low
  
  duration2 = pulseIn(echoPin2, HIGH); // read the duration of the pulse
  distance2 = (duration2/2) / 29.1; // calculate the distance in centimeters
  
  if (distance2 < full) { // if the distance is less than 20 cm
   digitalWrite(LED2, LOW); // set the trigPin low
   digitalWrite(LED1, HIGH); // set the trigPin low
   
  }
  else { // if the distance is greater than 20 cm
  digitalWrite(LED1, LOW); // set the trigPin low
   digitalWrite(LED2, HIGH); // set the trigPin low
  }
}