 
 
 void us_sensor(){
-  delay(50);
-
-  ultrasonic1 = sonar1.ping_cm();
-  ultrasonic2 = sonar2.ping_cm();
-
+  int i = 0, total1 = 0, total2 = 0,ave_us1 = 0, ave_us2 = 0;
+  for(i = 0; i < ULTRA_FREQUENCY; i++){
+    total1 += sonar1.ping_cm();
+    total2 += sonar2.ping_cm();
+  }
+  ave_us1 = total1 / ULTRA_FREQUENCY;
+  ave_us2 = total2 / ULTRA_FREQUENCY;
+  
+  ultrasonic1 = ave_us1;
+  ultrasonic2 = ave_us2;
+  /*
   if(ultrasonic1 < 20){
         Serial.print("1");
   }

    analogWrite(AMOTOR, motor_speed[0]);
    analogWrite(BMOTOR, motor_speed[1]);
}
void move_reverse(){
    digitalWrite(DIR_A, LOW);
    digitalWrite(DIR_B, LOW);

            case 'w':
              if(command_status == 1){
                //move_forward();
                //Serial.print("Hello1");
                variable_forward();
              }
              break;
              
            case 'r':
              if(command_status == 1){
                //move_reverse();
                //Serial.print("Hello2");
                variable_reverse();
              }
              break;

            case 'o':
              if(command_status == 1){
                //move_out();
                //Serial.print("Hello3");
                variable_out();
              }
              break;          

            case 'i':
              if(command_status == 1){
                //move_in();
                //Serial.print("Hello4");
                variable_in();
              } 
              break;
              
            case 'x':
              if(command_status == 1){
                //Serial.print("Hello5");
                stop_motor();
              }
              break;
              
            case 'u':
              if(command_status == 1){
                //Serial.println("Hello6");
                //noInterrupts();
                us_sensor();
                //us_sensor();
                //interrupts();
              }
              break;
              
            case 'n':
              if(command_status == 1){
                servo_info();
                //Serial.print("Hello7");
              }
              break;
              
            case 't':
              if(command_status == 1){
                servo_top();
                //Serial.print("Hello8");
              }
              break;
              
            case 'b':
              if(command_status == 1){
                //Serial.print("Hello12");
                servo_bottom();
              }
              break;
              
            case 'c':
              if(command_status == 1){
                servo_change();
                //Serial.print("Hello9");         
              }
            case '9':
              command_status = 1;
              //Serial.print("Hello10");
              break;
            default:
              //Serial.print("Hello11");
            break;
          }
      }
