В первых же строчках у вас:
IF ACTION_=FALSE
THEN KP:=KP*(-1);
ELSE KP:=KP;
END_IF
Уверен, вы хотели отрицательный KP, когда ACTION_ = FALSE, а этот код будет менять знак KP в каждом цикле ПЛК и оставлять знак случайным в тех циклах, когда ACTION_ = TRUE.