               }
               break;
             case 'C':
-              if(command_status == 1){
-                get_cap_value();
-              }
-              break;
+              //Serial.print("Cap");
+              if(command_status == 1){
+                cap_value();
+              }
+              break;
+            case 'H':
+              if(command_status == 1){
+                cap_hard_reset();
+              }
             
             default:
               //Serial.print("Hello11");

Servo myservo;
I2CEncoder encoder_B;
I2CEncoder encoder_A;
CapacitiveSensor capSense = CapacitiveSensor(CAP_SEND,CAP_REC); // (send_pin, receive_pin);




  Serial.print("&");
}

void cap_value(){
  
  long total = 0, cap_current = 0;
  long low_cap = 99999999, high_cap = 0, median_cap = 0;
  RunningMedian cap_array = RunningMedian(50);

  for (int i = 0, capAvg = 0; i < 100; i++){
    
    //Taking the Median
    cap_current = cap_sense.capacitiveSensorRaw(30);
    cap_array.add(cap_current);
    
    //Taking the lowest value
    if(low_cap > cap_current){
      low_cap = cap_current;
    }
    
    //Taking the highest value
    if(high_cap < cap_current){
      high_cap = cap_current;
    }
  }
  
  median_cap = int(cap_array.getMedian());
  
  Serial.print(high_cap);
  Serial.print('-');
  Serial.print(median_cap);
  Serial.print('-');
  Serial.print(low_cap);
  Serial.print('&');
}
void cap_hard_reset(){
  digitalWrite(CAP_SEND, LOW);
  delay(500);
}

void servo_info(){
