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