#include "Servo.h"
Servo myservo;
Servo myservo2;

int speed = 10;
int trigPin = 4;
int echoPin = 5;

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(3, OUTPUT);
  pinMode(6, OUTPUT); 

  myservo2.attach(10);
  myservo.attach(9); 
  
  digitalWrite(6, LOW);
  digitalWrite(3, HIGH); 
}

void loop() {
  long duration, distance;
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); 
  digitalWrite(trigPin, LOW); 

  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2)/29.1; 

 if (distance < 40) {
    myservo.write(-180);
    delay(speed);

    myservo2.write(180);
    delay(speed);
    
  } else {
    myservo.write(180);
    delay(speed);
    
    myservo2.write(180);
    delay(speed);
    
  }

  long x = digitalRead(16); 
  Serial.print(" ");
  Serial.print(x);
  Serial.print(" ");

  if (x==1) {
    myservo.write(-180);
    delay(1000);
    
    myservo2.write(-180);
    delay(1000);
  } else {
    myservo.write(-180);
    delay(speed);

    myservo2.write(180);
    delay(speed);
  }

}