я делал с пид. Только при пид 100% более какого-то времени... и далее ваш первый алгоритм.
ПИД заведен на одну группу. остальные вкл-выкл.

В вашем 2-м варианте заколебетесь пид настраивать.