#include <Servo.h>
int PIN_TRIG=3;
int PIN_ECHO=2;
Servo Leg1;
Servo Leg2;
Servo Hand;
int leg1=5;
int leg2=6;
int hand=9;
int a=0;
int b=0;
void setup() {
  Serial.begin(9600);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  Leg1.attach(leg1);
  Leg2.attach(leg2);
  Hand.attach(hand);
}
void loop() {
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  // Read the result:
  int time = pulseIn(PIN_ECHO, HIGH);
  Serial.print("TIME IN US: ");
  Serial.println(time);
  int distance=(time*0.034)/2;
  while(distance>=5 and distance<=100)
  {
      Serial.print("Distance in CM: ");
      Serial.println(distance);
    if(distance>=6 and distance<=100)
    {
      for(a=0;a<=181;a++)
      {
        Leg1.write(a);
        delay(10);
      }
      for(a=181;a>=-1;a--)
      {
        Leg1.write(a);
        delay(10);
      }
      delay(100);
      for(b=0;b<=180;b++)
      {
        Leg2.write(b);
        delay(10);
      }
      for(b=180;b>=0;b--)
      {
        Leg2.write(b);
        delay(10);
      }
    }
    if(distance>5)
    {
      distance=distance-1;
    }
    if(distance==5)
    {
      for(int c=90;c>=0;c--)
      {
        Hand.write(c);
        delay(5);
      }
      delay(4000);
      for(int c=0;c<=90;c++)
      {
        Hand.write(c);
        delay(5);
      }
    }
    break;
  }
  delay(1000);
}