#include <Servo.

h>

#define servo1Pin 6

#define servo2Pin 9

#define echoPin 11

#define trigPin 10

Servo myservo1;

Servo myservo2;

long duration;

int distance;

int disTreshold = 40;

int intPos1 = 115; //initial servo position

int pos1 = 75; //servo position if distance is >disTreshold

int intPos2 = 125; //initial servo position of servo 2

int pos2 = 50;

void setup() {

// put your setup code here, to run once:

Serial.begin(9600);

pinMode(trigPin, OUTPUT);

pinMode(echoPin, INPUT);

myservo1.attach(servo1Pin);

myservo2.attach(servo2Pin);

myservo1.write(intPos1);

myservo2.write(intPos2);

delay(2);
}

void loop() {

// put your main code here, to run repeatedly:

getDis();

if(distance>disTreshold){

myservo1.write(pos1);

myservo2.write(pos2);

}else if(distance<(15)){

myservo1.write(intPos1);

myservo2.write(intPos2);

delay(50);

void getDis(){

digitalWrite(trigPin, LOW);

delayMicroseconds(2);

digitalWrite(trigPin, HIGH);

delayMicroseconds(10);

digitalWrite(trigPin, LOW);

duration = pulseIn(echoPin, HIGH);

distance = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)

Serial.print("Distance: ");

Serial.print(distance);

Serial.println(" cm");