#include <Servo.h>

Servo servo_pin;
long distance1 = 0;
long distance2 = 0;

long readUltrasonicDistance(int triggerPin, int echoPin)  //Function for measure Distance
{
  pinMode(triggerPin, OUTPUT); 
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  return (pulseIn(echoPin, HIGH)*0.01723);
}
void setup() {
  // put your setup code here, to run once:
  servo_pin.attach(13);
  servo_pin.write(0);
  pinMode(13,OUTPUT);  // Set motor - pin 9 as an output
  pinMode(12, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(10, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  distance1 = readUltrasonicDistance(7, 8);
  distance2 = readUltrasonicDistance(5, 6);

  if (distance1<=100) {
    servo_pin.write(180);  //rotate motor 180 degree
    delay(1000);
  }

  else {
    servo_pin.write(0);  //rotate motor initial state
    delay(1000);
  }

  if (distance2 > 200) {
    digitalWrite(10, LOW);
    digitalWrite(11, HIGH);
    digitalWrite(12, LOW);
  }
  else if (distance2 <= 200 && distance2 > 100) {
    digitalWrite(10, HIGH);
    digitalWrite(11, LOW);
    digitalWrite(12, LOW);
  }
  else if (distance2 <= 100 && distance2 > 0) {
    digitalWrite(10, LOW);
    digitalWrite(11, LOW);
    digitalWrite(12, HIGH);
  }
}