то что вы слепили работать не будет
можно, как я думаю, приведя к соответствующему диапазону ограничивать выходную мощность (подавать туда инвертированный результат с ПИД подачи) регулятора обратки ПИД-регулятором подачи.