Да, выкинул его. Решил всё сделать на конечных автоматах. Очищаю по биту в 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;
}
/// Меняем мощность