полиномиальная аппроксимация

poly.cpp

millisec_poly = micros(); //замер времени
    if (charger_plugged == 1) { // зарядник подключен
    //зарядная кривая
        if ((charger_state_changed == 1) && (round(-486.8010*pow(Ubatt,4)+6961.1683*pow(Ubatt,3)-37045.1501*pow(Ubatt,2)+87023.3144*Ubatt-76186.5712) <= batt_percent_poly)) { // в случае переключения ждем пока процент вырастит, а потом меняем
            charger_state_changed = 1;
            if (millis() < 5000) { //обрабатываем запуск при подключенной зарядке. Без этого условия при запуске с подкл. зарядником batt_percent_poly вычисляться не будет и процент  зависнет.
              charger_state_changed = 0;
            }
        } else {
        charger_state_changed = 0;
        batt_percent_poly = round(-486.8010*pow(Ubatt,4)+6961.1683*pow(Ubatt,3)-37045.1501*pow(Ubatt,2)+87023.3144*Ubatt-76186.5712);
        if (Ubatt <= 3.54 ) {
           batt_percent_poly = map(Ubatt * 100, 300, 354, 0, 4);
        }
      }
    }
    if (charger_plugged == 0) { //зарядник не подключен
        // разрядная кривая
        if ((charger_state_changed == 0) && (round(17.8640*pow(Ubatt,5)-354.0801*pow(Ubatt,4)+2652.2137*pow(Ubatt,3)-9392.6518*pow(Ubatt,2)+15714.372*Ubatt-9877.3302) >= batt_percent_poly)) { // в случае переключения ждем пока процент упадет, а потом меняем
             charger_state_changed = 0;

       } else {

          batt_percent_poly =  round(17.8640*pow(Ubatt,5)-354.0801*pow(Ubatt,4)+2652.2137*pow(Ubatt,3)-9392.6518*pow(Ubatt,2)+15714.372*Ubatt-9877.3302);
          charger_state_changed = 1;
          if (Ubatt <= 3.26) {
               batt_percent_poly = map(Ubatt*100, 300, 326, 0, 3);//диапазон 3,0 вольта - 3,2В преобразуем в 0 - 3 процента.
          }
       }
    }
    if (batt_percent_poly > 100) { //проверяем результат аппроксимации. В некоторых точках может быть <0 или >100
        batt_percent_poly = 100;
    } else if (batt_percent_poly < 0) {
        batt_percent_poly = 0;
    } else if (Ubatt > 4.15 && Ubatt < 4.17) { //здесь мы немного обманываем пользователя. По факту 100% считается 4.2В, но такое напряжение не бывает после отключения зарядника. 
        batt_percent_poly = 98;
    } else if (Ubatt > 4.18 && Ubatt < 4.19) {
        batt_percent_poly = 99;
    } else if (Ubatt > 4.19) {
        batt_percent_poly = 100;
    }