Подскажите алгоритм

Да, выкинул его. Решил всё сделать на конечных автоматах. Очищаю по биту в StepMajor, пока мощность не станет выше установленной, затем ставлю биты, после последнего снятого, пока мощность не станет ниже установленной, тем самым нашёл диапазон, и уже по одному шагу тяну ближе всего к нужной мощности. Выглядит уже симпатично.

    enum class powerful {ADJUST, UP, DOWN, FIND, COMPLETE};
    powerful powerState;

    if (powerState < powerful::COMPLETE){
      PowerCalc += power;
      if (abs(power - npower) <= POWER_DELTA) {
        powerState = powerful::COMPLETE;
        Step = 3;
      } else if (power > npower){
        ((resetBit == 1) || (powerState == powerful::FIND) ? powerState = powerful::COMPLETE : powerState = powerful::DOWN);
      } else if ((power < npower) && (powerState == powerful::DOWN)) {
        powerState = powerful::FIND;
      }
      if(Step++ == 3){
        PowerCalc /= 3; 
        switch (powerState)
          {
          case powerful::ADJUST:
            StepMajor <<= 1;
            resetBit++;
            break;

          case powerful::DOWN:
            resetBit--;
            bitSet(StepMajor, resetBit-1);
            break;

          case powerful::FIND:
            StepMajor--;
            break;

          case powerful::COMPLETE:
            Serial.println("Adjust complete");
            break;
          
          default:
            break;
          }
        Step = 1;
        PowerCalc = 0;
        }
        /// Меняем мощность
1 лайк