#include <Servo.h>

#define trigPin 9
#define echoPin 8
int servoPin1 = 4; //arm 1
int servoPin2 = 5; //arm 2
int servoPin3 = 6; //gripper

Servo Servo1;
Servo Servo2;
Servo Servo3;
int sound = 250;

void setup() {
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
Servo1.attach(servoPin1); 
Servo1.write(0);
Servo2.attach(servoPin2); 
//Servo2.write(0);
Servo3.attach(servoPin3); 
Servo3.write(90);
}

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 < 80) {
Serial.print(distance);
Serial.println(" cm");
Servo1.write(75);
Servo2.write(75);
Servo3.write(180);

}

else if (distance<180) {
Serial.print(distance);
Serial.println(" cm");
Servo1.write(180); 
Servo2.write(160);
Servo3.write(10);
}
else {

Serial.println("The distance is more than 180cm");

}

delay(500);

}