               if(command_status == 1){
                 cap_hard_reset();
               }
+            case '+':
+              if(command_status == 1){
+                cap_test();
+              }
             
             default:
              // Serial.println("NULL");

int motor_speed[2];
int ultrasonic1 = 0, ultrasonic2 = 0;

//-------------------FUNCTIONs----------------
void stop_motor_ALL(){
  

  Serial.print("&");
}

void bubble_sort(long a[], int sizeofarray){
  
  //Serial.println("Begun BubbleSorting");
  long holder = 0;
    for(int i = 0; i < sizeofarray - i; i++){
      for(int j = 0; j < sizeofarray - (i+1); j++){
        if(a[j] > a[j+1]){
          //Serial.print("Value of ");
          //Serial.print(a[j]);
          //Serial.println(" has been moved up"); 
          holder = a[j];
          a[j] = a[j+1];
          a[j+1] = holder;
        }
      }
    }

  //Printing results
  /*
  for(int i = 0; i < sizeofarray; i++){
    Serial.print(a[i]);
    Serial.print(" ");
  }
  */
    //smallest ---- highest
}

long findMedian(long a[], int size){
  //Serial.println("Inside FindMedian");
  //Serial.print("Median: ");
  //Serial.println(a[round(size/2) - 1]);
  //to account for the zeroth element
  long returned_value = 0;
  size = size-1;
  /*
  Serial.print("Size: ");
  Serial.println(size);
  Serial.println("Median Location");
  Serial.print("With round: ");
  Serial.println(round(size/2));
  Serial.print("Without round: ");
  */
  float size_div = 0;
  size_div = float(size)/2;
  //Serial.println(size_div);
  
  if(size_div != round(size/2)){
    //Serial.println("Warning no normal Median found");
    returned_value = (a[round(size/2)] + a[round(size/2)+1])/2;
  }
  else
    returned_value = a[round(size/2)];
  
  return returned_value;
  
}

void cap_value(){
  
  int data_sample = 40;
  long total = 0, cap_current = 0;
  long cap_array_norm[data_sample], cap_array_min[data_sample], cap_array_max[data_sample];
  long min_median_cap = 0, max_median_cap = 0, median_cap = 0, ave_cap = 0;
  
  int median_location_first = 0;
  int median_location_last = 0;
  
  int first_flag = 0;
  int last_flag = 0;
  
  RunningMedian cap_array = RunningMedian(data_sample);
  RunningMedian cap_array_minside = RunningMedian(data_sample);
  RunningMedian cap_array_maxside = RunningMedian(data_sample);
  
  //Collecting Data
  //Serial.println("\n Iniciating Data Collection");
  
  for (int i = 0; i < data_sample; i++){
    cap_array_norm[i] = cap_sense.capacitiveSensorRaw(30);
    cap_array.add(cap_array_norm[i]);
    //Serial.print(i);
    //Serial.print(": ");
    //Serial.println(cap_array_norm[i]);
    }
    
  //Serial.println("Finished Collecting Data");
  //Serial.println("Printing the cap_array_norm");
  
  for(int l = 0; l < data_sample; l++){
    //Serial.print(l);
    //Serial.print(": ");
    //Serial.println(cap_array_norm[l]);
    ave_cap += cap_array_norm[l];
  }
  ave_cap /= data_sample;
  
 
  //Rearranged Data
  bubble_sort(cap_array_norm, data_sample);
  
  //Serial.println("Bubblesort Complete");

  //Serial.print("Average of Data: ");
  //Serial.println(ave_cap);
  
  
  median_cap = findMedian(cap_array_norm, data_sample);
  //Serial.print("Median with findMedian: ");
  //Serial.println(median_cap);
  
  //Serial.println("Found Median");
  /*
  for(int e = 0; e < median_location_first; e++){
    cap_array_minside.add(cap_array_norm[e]);
  }
  
  */
  //Serial.println("Printing the lower array");
  for(int e = 0; e < round(data_sample/2) ; e++){
    cap_array_min[e] = cap_array_norm[e];
    //Serial.print(cap_array_min[e]);
    //Serial.print(" ");
  }
  //Serial.print("\n");
  
  //min_median_cap = cap_array_minside.getMedian();
  
  min_median_cap = findMedian(cap_array_min, (data_sample / 2) );
  
  //Serial.print("Min Median: ");
  //Serial.println(min_median_cap);
  //Serial.println("Found min median");

  
  //Finding the median in the second section of the array
  
  //Serial.println("Printing the higher array");
  for(int w = round(data_sample/2); w < data_sample; w++){
    cap_array_max[w-round(data_sample/2)] = cap_array_norm[w]; 
    //Serial.print(cap_array_max[w-round(data_sample/2)]);
    //Serial.print(" ");  
  }
  //Serial.print("\n");

  max_median_cap = findMedian(cap_array_max, data_sample/2 );
  //Serial.println("Max Median: ");
  //Serial.println(max_median_cap);
  //Serial.println("Found max median");
  
  Serial.print(abs(max_median_cap));
  Serial.print('-');
  Serial.print(abs(median_cap));
  Serial.print('-');
  Serial.print(abs(min_median_cap));
  Serial.print('-');
  Serial.print(abs(ave_cap));
  Serial.print('&');
  
}
void cap_test(){
  for(int i = 0; i < 100; i++){
    Serial.println(cap_sense.capacitiveSensorRaw(30));
  }
}
void cap_hard_reset(){
  digitalWrite(CAP_SEND, LOW);
