void changeMode(byte i){
  Serial.print("Changing from: ");
  Serial.print(mode);
  Serial.print(" to: ");
  Serial.println(i);
  if(i==0){

  }else if(i==1){
    iniBLE(false);
  }else{
    leds[21] = DebugColor;
  }

  mode = i;
}

void nextMode(){
  byte newMode = mode+1;
  if(newMode>2) newMode = 0;
  changeMode(newMode);
}