You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
421 lines
6.8 KiB
421 lines
6.8 KiB
FUNCTION_BLOCK BS_PID
|
|
VAR_EXTERNAL
|
|
|
|
END_VAR
|
|
|
|
VAR_INPUT
|
|
RUN : BOOL ; (* PID鍔熻兘鍧楀惎鐢� *)
|
|
STOP_D : BOOL ; (* 寰�垎鍚�仠 *)
|
|
STOP_I : BOOL ; (* 绉�垎鍚�仠 *)
|
|
SET : REAL ; (* PID璋冭妭鐩�爣鍊� *)
|
|
FB : REAL ; (* PID璋冭妭鐩�爣鍊肩殑鍙嶉�鍊� *)
|
|
Kfb : REAL:=1.0; (* 鍙嶉�鍙傛暟 *)
|
|
Kp : REAL:=30.0 ; (* 姣斾緥鍙傛暟 *)
|
|
Ti : REAL:=150.0 ; (* 绉�垎鍙傛暟 *)
|
|
Td : REAL ; (* 寰�垎鍙傛暟 *)
|
|
Dead_Band : REAL; (* 姝诲尯 *)
|
|
Ymax : REAL ; (* 鏈€澶ц緭鍑哄€� *)
|
|
Ymin : REAL ; (* 鏈€灏忚緭鍑哄€� *)
|
|
END_VAR
|
|
|
|
VAR_OUTPUT
|
|
OUT : REAL ; (* 绉�垎鏈€澶ц緭鍑哄€� *)
|
|
END_VAR
|
|
|
|
VAR
|
|
TIME1 : REAL ;
|
|
TIME2 : REAL ;
|
|
Y1 : REAL ;
|
|
Y2 : REAL ;
|
|
Y3 : REAL ;
|
|
CYCL : BOOL ;
|
|
START : BOOL ;
|
|
Delta : REAL ;
|
|
OUTP : REAL ;
|
|
OUTI : REAL ;
|
|
OUTD : REAL ;
|
|
I : DINT ;
|
|
END_VAR
|
|
|
|
(*Automatically declared*)
|
|
VAR_EXTERNAL
|
|
END_VAR
|
|
(** {* **)
|
|
(** **)
|
|
(** 时 间：20191006 **)
|
|
(** 版 本：1.1 **)
|
|
(** 作 者：姚立 **)
|
|
(** 名 称：PID程序 **)
|
|
(** 说 明：用于PID控制计算 **)
|
|
(** 备 注：修正了除0情况的报错发生 **)
|
|
(** 依赖块：无 **)
|
|
(** **)
|
|
(** *} **)
|
|
(** **)
|
|
(** IF RUN=TRUE AND kp<>0.0 AND Ti<>0.0 THEN **)
|
|
(* if - Cond *)
|
|
LD RUN
|
|
EQ TRUE
|
|
AND ( kp
|
|
NE 0.0
|
|
)
|
|
AND ( Ti
|
|
NE 0.0
|
|
)
|
|
JMPCN ODK_SKIP_000000000000001_000000000000001
|
|
|
|
(* then - Stmts *)
|
|
|
|
(** IF (TIME2<>(DINT_TO_REAL(TIME_TO_DINT(GetTime(t#0ms))))) THEN **)
|
|
(* if - Cond *)
|
|
LD TIME2
|
|
NE ( t#0ms
|
|
GetTime
|
|
TIME_TO_DINT
|
|
DINT_TO_REAL
|
|
)
|
|
JMPCN ODK_SKIP_000000000000002_000000000000001
|
|
|
|
(* then - Stmts *)
|
|
|
|
(** TIME1:=TIME2; **)
|
|
(* assign - Stmt *)
|
|
LD TIME2
|
|
ST TIME1
|
|
|
|
(** TIME2:=DINT_TO_REAL(TIME_TO_DINT(GetTime(t#0ms))); **)
|
|
(* assign - Stmt *)
|
|
LD t#0ms
|
|
GetTime
|
|
TIME_TO_DINT
|
|
DINT_TO_REAL
|
|
ST TIME2
|
|
|
|
(** Y1:=Y2; **)
|
|
(* assign - Stmt *)
|
|
LD Y2
|
|
ST Y1
|
|
|
|
(** Y2:=Y3; **)
|
|
(* assign - Stmt *)
|
|
LD Y3
|
|
ST Y2
|
|
|
|
(** Y3:=SET-(Kfb*FB); **)
|
|
(* assign - Stmt *)
|
|
LD SET
|
|
SUB ( Kfb
|
|
MUL FB
|
|
)
|
|
ST Y3
|
|
|
|
(** IF I>=2 AND DEAD_BAND<ABS(Y3) THEN{*死区*} **)
|
|
(* if - Cond *)
|
|
LD I
|
|
GE 2
|
|
AND ( DEAD_BAND
|
|
LT ( Y3
|
|
ABS
|
|
)
|
|
)
|
|
JMPCN ODK_SKIP_000000000000003_000000000000001
|
|
|
|
(* then - Stmts *)
|
|
|
|
(** OUTP:=Y3*Kp; **)
|
|
(* assign - Stmt *)
|
|
LD Y3
|
|
MUL kp
|
|
ST OUTP
|
|
|
|
(** Delta:=(TIME2-TIME1)/1000.0; **)
|
|
(* assign - Stmt *)
|
|
LD TIME2
|
|
SUB TIME1
|
|
DIV 1000.0
|
|
ST Delta
|
|
|
|
(** IF STOP_I<>TRUE THEN **)
|
|
(* if - Cond *)
|
|
LD STOP_I
|
|
NE TRUE
|
|
JMPCN ODK_SKIP_000000000000004_000000000000001
|
|
|
|
(* then - Stmts *)
|
|
|
|
(** OUTI:=OUTI+((Y2+Y3)*Delta/2.0)/Ti; **)
|
|
(* assign - Stmt *)
|
|
LD OUTI
|
|
ADD ( Y2
|
|
ADD Y3
|
|
MUL Delta
|
|
DIV 2.0
|
|
DIV Ti
|
|
)
|
|
ST OUTI
|
|
|
|
(** IF OUTI>Ymax THEN{*防止积分过大*} **)
|
|
(* if - Cond *)
|
|
LD OUTI
|
|
GT Ymax
|
|
JMPCN ODK_SKIP_000000000000005_000000000000001
|
|
|
|
(* then - Stmts *)
|
|
|
|
(** OUTI:=Ymax; **)
|
|
(* assign - Stmt *)
|
|
LD Ymax
|
|
ST OUTI
|
|
|
|
JMP ODK_END_IF_000000000000005
|
|
|
|
(** ELSIF OUTI<(0.0-Ymax) THEN **)
|
|
(* elseif - Cond *)
|
|
ODK_SKIP_000000000000005_000000000000001:
|
|
LD OUTI
|
|
LT ( 0.0
|
|
SUB Ymax
|
|
)
|
|
JMPCN ODK_SKIP_000000000000005_000000000000002
|
|
|
|
(* elsif - Stmts *)
|
|
|
|
(** OUTI:=(0.0-Ymax); **)
|
|
(* assign - Stmt *)
|
|
LD 0.0
|
|
SUB Ymax
|
|
ST OUTI
|
|
|
|
JMP ODK_END_IF_000000000000005
|
|
|
|
(** END_IF; **)
|
|
(* if - End *)
|
|
ODK_SKIP_000000000000005_000000000000002:
|
|
ODK_END_IF_000000000000005:
|
|
|
|
JMP ODK_END_IF_000000000000004
|
|
|
|
(** END_IF; **)
|
|
(* if - End *)
|
|
ODK_SKIP_000000000000004_000000000000001:
|
|
ODK_END_IF_000000000000004:
|
|
|
|
(** IF STOP_D<>TRUE THEN **)
|
|
(* if - Cond *)
|
|
LD STOP_D
|
|
NE TRUE
|
|
JMPCN ODK_SKIP_000000000000006_000000000000001
|
|
|
|
(* then - Stmts *)
|
|
|
|
(** OUTD:=Td*(0.5*Y1-2.0*Y2+1.5*Y3)/Delta; **)
|
|
(* assign - Stmt *)
|
|
LD Td
|
|
MUL ( 0.5
|
|
MUL Y1
|
|
SUB ( 2.0
|
|
MUL Y2
|
|
)
|
|
ADD ( 1.5
|
|
MUL Y3
|
|
)
|
|
)
|
|
DIV Delta
|
|
ST OUTD
|
|
|
|
JMP ODK_END_IF_000000000000006
|
|
|
|
(** END_IF; **)
|
|
(* if - End *)
|
|
ODK_SKIP_000000000000006_000000000000001:
|
|
ODK_END_IF_000000000000006:
|
|
|
|
JMP ODK_END_IF_000000000000003
|
|
|
|
(** ELSE **)
|
|
(* else *)
|
|
ODK_SKIP_000000000000003_000000000000001:
|
|
|
|
(* else - Stmts *)
|
|
|
|
(** I:=I+1; **)
|
|
(* assign - Stmt *)
|
|
LD I
|
|
ADD 1
|
|
ST I
|
|
|
|
(** END_IF; **)
|
|
(* if - End *)
|
|
ODK_END_IF_000000000000003:
|
|
|
|
JMP ODK_END_IF_000000000000002
|
|
|
|
(** END_IF; **)
|
|
(* if - End *)
|
|
ODK_SKIP_000000000000002_000000000000001:
|
|
ODK_END_IF_000000000000002:
|
|
|
|
(** {* Adder *} **)
|
|
(** IF OUTP+OUTI+OUTD>Ymax THEN **)
|
|
(* if - Cond *)
|
|
LD OUTP
|
|
ADD OUTI
|
|
ADD OUTD
|
|
GT Ymax
|
|
JMPCN ODK_SKIP_000000000000007_000000000000001
|
|
|
|
(* then - Stmts *)
|
|
|
|
(** OUT:=Ymax; **)
|
|
(* assign - Stmt *)
|
|
LD Ymax
|
|
ST OUT
|
|
|
|
JMP ODK_END_IF_000000000000007
|
|
|
|
(** ELSIF OUTP+OUTI+OUTD<Ymin THEN **)
|
|
(* elseif - Cond *)
|
|
ODK_SKIP_000000000000007_000000000000001:
|
|
LD OUTP
|
|
ADD OUTI
|
|
ADD OUTD
|
|
LT Ymin
|
|
JMPCN ODK_SKIP_000000000000007_000000000000002
|
|
|
|
(* elsif - Stmts *)
|
|
|
|
(** OUT:=Ymin; **)
|
|
(* assign - Stmt *)
|
|
LD Ymin
|
|
ST OUT
|
|
|
|
JMP ODK_END_IF_000000000000007
|
|
|
|
(** ELSE **)
|
|
(* else *)
|
|
ODK_SKIP_000000000000007_000000000000002:
|
|
|
|
(* else - Stmts *)
|
|
|
|
(** OUT:=OUTP+OUTI+OUTD; **)
|
|
(* assign - Stmt *)
|
|
LD OUTP
|
|
ADD OUTI
|
|
ADD OUTD
|
|
ST OUT
|
|
|
|
(** END_IF; **)
|
|
(* if - End *)
|
|
ODK_END_IF_000000000000007:
|
|
|
|
JMP ODK_END_IF_000000000000001
|
|
|
|
(** **)
|
|
(** ELSE **)
|
|
(* else *)
|
|
ODK_SKIP_000000000000001_000000000000001:
|
|
|
|
(* else - Stmts *)
|
|
|
|
(** TIME1:=0.0; **)
|
|
(* assign - Stmt *)
|
|
LD 0.0
|
|
ST TIME1
|
|
|
|
(** TIME2:=0.0; **)
|
|
(* assign - Stmt *)
|
|
LD 0.0
|
|
ST TIME2
|
|
|
|
(** Y1:=0.0; **)
|
|
(* assign - Stmt *)
|
|
LD 0.0
|
|
ST Y1
|
|
|
|
(** Y2:=0.0; **)
|
|
(* assign - Stmt *)
|
|
LD 0.0
|
|
ST Y2
|
|
|
|
(** Y3:=0.0; **)
|
|
(* assign - Stmt *)
|
|
LD 0.0
|
|
ST Y3
|
|
|
|
(** CYCL:=FALSE; **)
|
|
(* assign - Stmt *)
|
|
LD FALSE
|
|
ST CYCL
|
|
|
|
(** START:=FALSE; **)
|
|
(* assign - Stmt *)
|
|
LD FALSE
|
|
ST START
|
|
|
|
(** Delta:=0.0; **)
|
|
(* assign - Stmt *)
|
|
LD 0.0
|
|
ST Delta
|
|
|
|
(** OUTP:=0.0; **)
|
|
(* assign - Stmt *)
|
|
LD 0.0
|
|
ST OUTP
|
|
|
|
(** OUTI:=0.0; **)
|
|
(* assign - Stmt *)
|
|
LD 0.0
|
|
ST OUTI
|
|
|
|
(** OUTD:=0.0; **)
|
|
(* assign - Stmt *)
|
|
LD 0.0
|
|
ST OUTD
|
|
|
|
(** OUT :=0.0; **)
|
|
(* assign - Stmt *)
|
|
LD 0.0
|
|
ST OUT
|
|
|
|
(** I :=0; **)
|
|
(* assign - Stmt *)
|
|
LD 0
|
|
ST I
|
|
|
|
(** END_IF; **)
|
|
(* if - End *)
|
|
ODK_END_IF_000000000000001:
|
|
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
(** **)
|
|
|
|
END_FUNCTION_BLOCK
|