//-------------------- Функция программного сброса(перевод стека в начало програмного кода) -----------
void (*resetFunc)(void) = 0;//Программа начинает исполнятся с самого начала
//-----------------------------------------------------------------------------------------------------
if(Serial2.available()>0){// Если есть данные принятые от дисплея, то
String inString; // Объявляем строку для получения этих данных
int intVal=0;
int intVal2=0;
while (Serial2.available()) {
char inChar = Serial2.read(); delay(2);
inString += inChar;
//if (inChar=='\n')inString="";//конец строки - обнуляем строку
if (inChar == '#') {
String ID_command = getValue(inString, ':', 0);// ID команды
String val = getValue(inString, ':', 1);// значение
String sub_val = getValue(inString, ':', 2);// subID команды
intVal = val.toInt();// перевод строки в числовое значение
if(sub_val=="#")Serial.println("No sub_val!!! ");
else{
intVal2 = sub_val.toInt();// перевод строки в числовое значение
Serial.print("sub_val: ");
Serial.println(intVal2,DEC);}
inString="";
if(ID_command.equals("LAdr")){// если получили адрес лока, записываем его в переменную адреса
LocAddress = intVal;
Speed = intVal2;
notifyz21LocoSpeed(LocAddress, Speed, 28);
}
else if(ID_command.equals("FL")){// если получили команду вкл/выкл света
//Func_State = intVal;
notifyz21LocoFkt(LocAddress, intVal, 0);
}
else if(ID_command.equals("F")){// если получили команду вкл/выкл F
Serial.print("F");
Serial.print(intVal,DEC);// номер функции
Serial.print(" state ");
Serial.println(intVal2,DEC);// состояние функции
notifyz21LocoFkt(LocAddress, intVal2, intVal);
}
else if(ID_command.equals("ON")){// если получили команду "ON"
globalPower(csNormal);//
}
else if(ID_command.equals("OFF")){// если получили команду "OFF"
globalPower(csTrackVoltageOff);//
}
else if(ID_command.equals("ESTOP")){// если получили команду "ESTOP"
globalPower(csEmergencyStop);//
}
else if(ID_command.equals("RESET")){// если получили команду "RESET"
page_0();//отправляем команду дисплею, для отображения стартовой заставки
resetFunc();//вызываем функцию программного сброса
}
else if(ID_command.equals("ReadCV")){// если получили команду чтения CV
CV = intVal;
notifyz21CVREAD(0, CV-1);//читаем CV
}
else if(ID_command.equals("WriteCV")){// если получили команду записи CV
CV = intVal;
notifyz21CVWRITE(0, CV-1, intVal2);//пишем CV
}
else if(ID_command.equals("SW_R")){//
notifyz21Accessory(intVal, 1, 1);
}
else if(ID_command.equals("SW_G")){//
notifyz21Accessory(intVal, 0, 1);
}
}
// else if(inChar == '&'){
//Serial.print("RGB_Command: ");
//Serial.println(inString);}
}
}

// Функция парсинга строки
String getValue(String data, char separator, int index)
{
int found = 0;
int strIndex[] = {0, -1 };
int maxIndex = data.length()-1;
for(int i=0; i<=maxIndex && found<=index; i++){
if(data.charAt(i)==separator || i==maxIndex){
found++;
strIndex[0] = strIndex[1]+1;
strIndex[1] = (i == maxIndex) ? i+1 : i;}
}
return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
}