функциональный блок для управления перекидным клапаном

[alexval2006]

добрый вечер всем написал функциональный блок для управления перекидным клапаном это двух позиционный механизм приводимый в движение электромотором 380В, конечные положения определяющийся индуктивными датчиками сенсоровскими ВБИ. Имеется два режима работы ручной и автомат. В ручном режиме оператор нажимает экранные кнопки в скаде входы A:BOOL; (*Кнопка положение A*) B:BOOL; (*Кнопка положение B*) после чего запускаться таймер настроенный на время перехода механизма от одного в другое конечное положение иначе авария. Выходы OUT_A:BOOL; (*Выход положение A*) OUT_B:BOOL; (*Выход положение B*) управляют реверсивным магнитным пускателем электродвигателя. В автоматическом режиме сигнал AVTO:BOOL; (*Автоматический режим работы FALSE-ручное TRUE-автомат*) механизм управляться сигналами C:BOOL; (*РЕЖИМ АВТОМАТ положение A*) D:BOOL; (*РЕЖИМ АВТОМАТ положение B*) это сигналы от технологического процесса различные механизмы с которыми связан этот клапан.

В общем есть две проблемы первая не могу добиться нормальной работы с тех пор как добавил автоматический режим вроде и правильно все но хз. Вторая как оптимизировать код может что то улучшить а то у меня ощущение что я написал индусский код.
************************************************** *******
FUNCTION_BLOCK PK(*Функциональный блок для управления перекидным клапаном*)
(*двух позиционный механизм*)
VAR_INPUT(*Входные переменные датчики кнопки и тп*)
AVTO:BOOL; (*Автоматический режим работы FALSE-ручное TRUE-автомат*)
A:BOOL; (*Кнопка положение A*)
B:BOOL; (*Кнопка положение B*)
C:BOOL; (*РЕЖИМ АВТОМАТ положение A*)
D:BOOL; (*РЕЖИМ АВТОМАТ положение B*)
SENSOR_A:BOOL; (*Датчик положение A*)
SENSOR_B:BOOL; (*Датчик положение B*)
ALARM_RES: BOOL; (*Сигнал сброса аварии*)
END_VAR
VAR_OUTPUT(*выходные переменные пускатели аварии и тп*)
OUT_A:BOOL; (*Выход положение A*)
OUT_B:BOOL; (*Выход положение B*)
ALARM_OUT:BOOL; (*Выход авария перекидного клапана*)
END_VAR
VAR(*Внутрении переменные для собственных нужд ФБ таймеры и прочее*)
tim1:TON;
tim2:TON;
END_VAR

************************************************** ****
IF AVTO THEN
(*-------------------------------------------------------------------------*)
IF C=TRUE AND D=FALSE THEN
OUT_A:=TRUE;
END_IF

IF C=FALSE AND D=TRUE THEN
OUT_B:=TRUE;
END_IF
(*-------------------------------------------------------------------------*)
ELSE
(*-------------------------------------------------------------------------*)
IF A=TRUE AND B=FALSE THEN
OUT_A:=TRUE;
END_IF

IF A=FALSE AND B=TRUE THEN
OUT_B:=TRUE;
END_IF

(*-------------------------------------------------------------------------*)
END_IF

IF OUT_A=TRUE THEN
tim1(IN:=OUT_A, PT:=T#5s);
ALARM_OUT:=tim1.Q;
END_IF

IF SENSOR_A=TRUE THEN
OUT_A:=FALSE;
END_IF
(*-------------------------------------------------------------------------*)
IF OUT_B=TRUE THEN
tim2(IN:=OUT_B, PT:=T#5s);
ALARM_OUT:=tim2.Q;
END_IF

IF SENSOR_B=TRUE THEN
OUT_B:=FALSE;
END_IF

IF OUT_A=TRUE THEN
tim1(IN:=OUT_A, PT:=T#5s);
ALARM_OUT:=tim1.Q;
END_IF

IF SENSOR_A=TRUE THEN
OUT_A:=FALSE;
END_IF
(*-------------------------------------------------------------------------*)
IF OUT_B=TRUE THEN
tim2(IN:=OUT_B, PT:=T#5s);
ALARM_OUT:=tim2.Q;
END_IF

IF SENSOR_B=TRUE THEN
OUT_B:=FALSE;
END_IF


(*-------------------------------------------------------------------------*)
IF OUT_A=TRUE AND OUT_B=TRUE THEN
ALARM_OUT:=TRUE ;
END_IF
(*-------------------------------------------------------------------------*)
IF ALARM_OUT=TRUE THEN
OUT_A:=FALSE;
OUT_B:=FALSE;
END_IF
(*-------------------------------------------------------------------------*)
IF ALARM_RES=TRUE THEN
ALARM_OUT:=FALSE;
tim1(IN:=FALSE, PT:=T#5s);
tim2(IN:=FALSE, PT:=T#5s);
OUT_A:=FALSE;
OUT_B:=FALSE;
END_IF