unsigned long T = 0;      
unsigned long L = 0;      
unsigned long x=1000;
int f = 10;   
int s = 0;   
int u = 0;   
int k = 0;   
int z = 0;   
int g = 90;   
   
#include <Servo.h>
Servo myservo;  

void setup() 
{  
   myservo.attach(6);  
   myservo.write(90);
   g=90;
   delay(1000);   

   pinMode(30, OUTPUT);   
   pinMode(31, INPUT);   
   pinMode(40, OUTPUT);
   pinMode(41, OUTPUT);
   pinMode(42, OUTPUT);
   pinMode(43, OUTPUT);
   pinMode(2, OUTPUT);
   pinMode(3, OUTPUT);
   pinMode(4, OUTPUT);
   pinMode(5, OUTPUT);
   pinMode(8, OUTPUT);
   pinMode(9, OUTPUT);
   pinMode(11, OUTPUT);
   pinMode(12, OUTPUT);
   pinMode(22, OUTPUT);
   pinMode(24, OUTPUT);
   pinMode(28, OUTPUT);
   pinMode(32, OUTPUT);
   pinMode(36, OUTPUT);
   pinMode(46, OUTPUT);
   pinMode(50, OUTPUT);
   pinMode(52, OUTPUT);

   digitalWrite(2, LOW);
   digitalWrite(3, LOW);
   digitalWrite(4, LOW);
   digitalWrite(5, LOW);
   digitalWrite(8, LOW);
   digitalWrite(9, LOW);
   digitalWrite(11, LOW);
   digitalWrite(12, LOW);
   digitalWrite(22, LOW);
   digitalWrite(24, LOW);
   digitalWrite(32, LOW);
   digitalWrite(36, LOW);
   digitalWrite(46, LOW);
   digitalWrite(50, LOW);
   digitalWrite(52, LOW);
   Serial.begin(9600);
}

void loop() 
  {
  if (k==0)
    {
    digitalWrite(2, HIGH);
    delay(x);
    digitalWrite(2, LOW);
    digitalWrite(3, HIGH);
    delay(x);
    digitalWrite(3, LOW);
    digitalWrite(4, HIGH);
    delay(x);
    digitalWrite(4, LOW);
    digitalWrite(5, HIGH);
    delay(x);
    digitalWrite(5, LOW);
    digitalWrite(8, HIGH);
    delay(x);
    digitalWrite(8, LOW);
    digitalWrite(9, HIGH);
    delay(x);
    digitalWrite(9, LOW);
    digitalWrite(11, HIGH);
    delay(x);
    digitalWrite(11, LOW);
    digitalWrite(12, HIGH);
    delay(x);
    digitalWrite(12, LOW);
    digitalWrite(22, HIGH);
    delay(x);
    digitalWrite(22, LOW);
    digitalWrite(24, HIGH);
    delay(x);
    digitalWrite(24, LOW);
    digitalWrite(28, HIGH);
    delay(x);
    digitalWrite(28, LOW);
    digitalWrite(32, HIGH);
    delay(x);
    digitalWrite(32, LOW);
    digitalWrite(36, HIGH);
    delay(x);
    digitalWrite(36, LOW);
    digitalWrite(46, HIGH);
    delay(x);
    digitalWrite(46, LOW);
    digitalWrite(50, HIGH);
    delay(x);
    digitalWrite(50, LOW);
    digitalWrite(52, HIGH);
    delay(x);
    digitalWrite(52, LOW);
    k=1;
    }

    else if (k==1)
      {
      for (int i=1; i <= f; i++)
          {
          x=100;  
          digitalWrite(2, HIGH);
          digitalWrite(22, HIGH);
          delay(x);
          digitalWrite(2, LOW);
          digitalWrite(22, LOW);
          digitalWrite(3, HIGH);
          digitalWrite(24, HIGH);
          delay(x);
          digitalWrite(3, LOW);
          digitalWrite(24, LOW);
          digitalWrite(4, HIGH);
          digitalWrite(28, HIGH);
          delay(x);
          digitalWrite(4, LOW);
          digitalWrite(28, LOW);
          digitalWrite(5, HIGH);
          digitalWrite(32, HIGH);
          delay(x);
          digitalWrite(5, LOW);
          digitalWrite(32, LOW);
          digitalWrite(8, HIGH);
          digitalWrite(36, HIGH);
          delay(x);
          digitalWrite(8, LOW);
          digitalWrite(36, LOW);
          digitalWrite(9, HIGH);
          digitalWrite(46, HIGH);
          delay(x);
          digitalWrite(9, LOW);
          digitalWrite(46, LOW);
          digitalWrite(11, HIGH);
          digitalWrite(50, HIGH);
          delay(x);
          digitalWrite(11, LOW);
          digitalWrite(50, LOW);
          digitalWrite(12, HIGH);
          digitalWrite(52, HIGH);
          delay(x);
          digitalWrite(12, LOW);
          digitalWrite(52, LOW);
          delay(x);
          }
      k=2;
     
      }

    else if (k==2 && u==0)  
        {
        digitalWrite(30, HIGH);                     
        delayMicroseconds(50);                        
        digitalWrite(30, LOW);  
        T = pulseIn(31, HIGH);   
        u=1;                     
        L = (T*340)/2;  
        L=L/10000; 
        Serial.println(L);
                       
        if(L < 30) {k=50;} else {k=1; f=1;}
        }

    else if (k==2 && u==1 && g==90)
        {           
        myservo.write(20);             
        delay(900);        
        k=2;
        u=0;
        g=20;
        }
   
    else if (k==2 && u==1 && g==20)
        {           
        myservo.write(89);             
        delay(900);       
        k=2;
        u=0;
        g=89;
        }
   
    else if (k==2 && u==1 && g==89)
        {           
        myservo.write(160);             
        delay(900);      
        k=2;
        u=0;
        g=160;
        }
   
    else if (k==2 && u==1 && g==160)
        {           
        myservo.write(90);             
        delay(900);      
        k=1;
        f=10;
        u=0;
        g=90;
        }

     else if (k==50)
        { 
        for (int i=1; i <= 900; i++)
            {

            if (i<100){ digitalWrite(2, HIGH); digitalWrite(22, HIGH);}  
            else if (i<200){ digitalWrite(3, HIGH); digitalWrite(24, HIGH);
                            digitalWrite(2, LOW); digitalWrite(22, LOW);}
            else if (i<300){ digitalWrite(4, HIGH); digitalWrite(28, HIGH);
                            digitalWrite(3, LOW); digitalWrite(24, LOW);}
            else if (i<400){ digitalWrite(5, HIGH); digitalWrite(32, HIGH);
                            digitalWrite(4, LOW); digitalWrite(28, LOW);}
            else if (i<5000){ digitalWrite(8, HIGH); digitalWrite(36, HIGH);
                            digitalWrite(5, LOW); digitalWrite(32, LOW);}
            else if (i<600){ digitalWrite(9, HIGH); digitalWrite(46, HIGH);
                            digitalWrite(8, LOW); digitalWrite(36, LOW);}
            else if (i<700){ digitalWrite(11, HIGH); digitalWrite(50, HIGH);
                            digitalWrite(9, LOW); digitalWrite(46, LOW);}
            else if (i<800){ digitalWrite(12, HIGH); digitalWrite(52, HIGH);
                            digitalWrite(11, LOW); digitalWrite(50, LOW);}
            else if (i<900){digitalWrite(12, LOW); digitalWrite(52, LOW);}
            switch(s){
            case 0:
            digitalWrite(40, HIGH);
            digitalWrite(41, LOW);
            digitalWrite(42, LOW);
            digitalWrite(43, LOW);
            break;
            case 1:
            digitalWrite(40, LOW);
            digitalWrite(41, HIGH);
            digitalWrite(42, LOW);
            digitalWrite(43, LOW);
            break;
            case 2:
            digitalWrite(40, LOW);
            digitalWrite(41, LOW);
            digitalWrite(42, HIGH);
            digitalWrite(43, LOW);
            break;
            case 3:
            digitalWrite(40, LOW);
            digitalWrite(41, LOW);
            digitalWrite(42, LOW);
            digitalWrite(43, HIGH);
            break;}   
            s++;
            if(s > 3){ s = 0;  }
            delay(2);  
            }
        k=1;
        }
   }