关于泵轮换程序的修改，ST结构化文本语言

我这有一段程序如下：
h:=0;
WHILE h<=i_i_Pump_Sys_Nb DO
 IF  i_r_Water_level<=i_r_Close_LowertWater THEN Sys_Machine_Req_Nb:=0;
    ELSE IF i_r_Water_level>=i_arr_di_Pump_level[i_i_Pump_Sys_Nb] THEN Sys_Machine_Req_Nb: = i_i_Pump_Sys_Nb ;
          ELSE  IF i_r_Water_level<=i_arr_di_Pump_level[h+1]-i_r_WaterSupply_Setpoint AND i_r_Water_level>=i_arr_di_Pump_level[h] THEN
Sys_Machine_Req_Nb:=h;
 END_IF
END_IF;
END_IF;
 h:=h+1;
END_WHILE;

这是一段循环语句，但是有一点，实际泵数是6，如果按照上属语句，循环时会一直到6，也就是只要到达6台泵的运行水位，6台泵就会都运行，我现在想不管任何水位下都只需要最多四台泵运行，应该如何修改，请求高手帮忙解答下。

=i_i_Pump_Sys_Nb系统水泵设置台数
i_r_Close_LowertWater关泵水位
i_r_Water_level 系统实际水位
i_arr_di_Pump_level开泵水位
Sys_Machine_Req_Nb需要开泵数
i_r_WaterSupply_Setpoint系统关泵液位差，
我觉得应该从紫色标记那一句入手，但改来改去都是适得其反。

我的目的再说一遍是：实际6台泵，我只想让最多四台泵同时运行，6台泵轮换工作。

紫色这一句如果能将最大可运行泵数4作为一个限制条件提出来。。。实在不会。

这个没有知道的？