?? 14.2.1位置式pid控制.txt
字號:
DATA EQU 30H
PSTNPID:
MOV R1,#DATA
MOV R0,#COEFRK
SETB 3AH
MOV A,@R0
MOV R6,A
INC R0
MOV A,@R0
MOV R2,A
INC R0
MOV A,@R0
MOV R3,A
DEC R0
DEC R0
MOV A,@R1
MOV R4,A
INC R1
MOV A,@R1
DEC R1
DEC R1
MOV A,R6
MOV C,ACC.7
MOV 38H,C
XRL A,R7
JNB ACC.7,FA1
CPL 3AH
FA1:
MOV A,R6
MOV C,ACC.6
MOV ACC.7,C
MOV R6,A
MOV A,R7
MOV C,ACC.6
MOV ACC.7,C
MOV R7,A
CLR C
MOV A,R6
SUBB A,R7
JZ FA2
CLR PWS.5
CLR 39H
JB ACC.7,FA5
CJNE R4,#0,FA6
CJNE R5,#0,FA6
FA2:
JB 3AH,FA8
MOV A,R3
ADD A,R5
MOV R3,A
MOV A,R2
ADDC A,R4
MOV R2,A
JNC FA4
SETB 39H
CLR C
FA3:
CLR PWS.5
FSDT:
JC FS2
MOV C,39H
JB PWS.5,FS1
MOV A,R2
RRC A
MOV R2,A
MOV A,R3
RRC A
MOV R3,A
INC R6
RET
FS1:
MOV A,R4
RRC A
MOV R4,A
MOV A,R5
RRC A
MOV R5,A
INC R7
RET
FS2:
MOV A,R2
JNZ FS4
CJNE R3,#0,FS5
MOV R6,#41H
FS3:
RET
FS4:
JB ACC.7,FS3
FS5:
MOV C,PWS.5
MOV A,R3
RLC A
MOV R3,A
MOV A,R2
RLC A
MOV R2,A
CLR PWS.5
DEC R6
SJMP FS2
FA4:
CJNE R2,#0,.FAA
CJNE R3,#0,FAA
MOV R4,#41H
RET
FAA:
MOV A,R6
MOV C,38H
MOV ACC.7,C
XCH A,R4
MOV R6,A
RET
FA5:
CJNE R2,#0,.FA7
CJNE R3,#0,FA7
MOV A,R7
MOV R6,A
SJMP FA2
FA6:
CPL PWS.5
FA7:
CLR C
JC FS2
MOV C,39H
JB PWS.5,FS6
MOV A,R2
RRC A
MOV R2,A
MOV A,R3
RRC A
MOV R3,A
INC R6
RET
FS6:
MOV A,R4
RRC A
MOV R4,A
MOV A,R5
RRC A
MOV R5,A
INC R7
RET
FS7:
MOV A,R2
JNZ FS4
CJNE R3,#0,FS10
MOV R6,#41H
FS8:
RET
FS9:
JB ACC.7,FS8
FS10:
MOV C,PWS.5
MOV A,R3
RLC A
MOV R3,A
MOV A,R2
RLC A
MOV R2,A
CLR PWS.5
DEC R6
SJMP FS7
SJMP FA1
FA8:
MOV A,R3
CLR C
SUBB A,R5
MOV R3,A
MOV A,R2
SUBB A,R4
MOV R2,A
JNC FA9
CLR A
CLR C
SUBB A,R3
MOV R3,A
CLR A
SUBB A,R2
MOV R2,A
CPL 38H
SETB C
SJMP FA3
MOV Rl,#BIASE.0
LCALL FSTR
MOV R0,#COEFKP
LCALL FMUL
MOV R1,#BIASPP
LCALL FSTR
MOV R0,#BIASED
MOV R1,#COEFKI
LCALL FMUL
MOV R1,#MIDDLE
LCALL FSTR
MOV R0,#BIASPI
LCALL FADD
MOV R1,#BIASPI
LCALL FSTR
MOV R0,#BIASED
MOV R1,#BIASE1
MOV A,R3
ADD A,R5
MOV R3,A
MOV A,R2
ADDC A,R4
MOV R2,A
JNC FA4
SETB 39H
CLR C
FA3:
CLR PWS.5
FSDT:
JC FS2
MOV C,39H
JB PWS.5,FS1
MOV A,R2
RRC A
MOV R2,A
MOV A,R3
RRC A
MOV R3,A
INC R6
RET
FS1:
MOV A,R4
RRC A
MOV R4,A
MOV A,R5
RRC A
MOV R5,A
INC R7
RET
FS2:
MOV A,R2
JNZ FS4
CJNE R3,#0,FS5
MOV R6,#41H
FS3:
RET
FS4:
JB ACC.7,FS3
FS5:
MOV C,PWS.5
MOV A,R3
RLC A
MOV R3,A
MOV A,R2
RLC A
MOV R2,A
CLR PWS.5
DEC R6
SJMP FS2
FA4:
CJNE R2,#0,.FAA
CJNE R3,#0,FAA
MOV R4,#41H
RET
FAA:
MOV A,R6
MOV C,38H
MOV ACC.7,C
XCH A,R4
MOV R6,A
RET
FA5:
CJNE R2,#0,.FA7
CJNE R3,#0,FA7
MOV A,R7
MOV R6,A
SJMP FA2
FA6:
CPL PWS.5
FA7:
CLR C
JC FS2
MOV C,39H
JB PWS.5,FS6
MOV A,R2
RRC A
MOV R2,A
MOV A,R3
RRC A
MOV R3,A
INC R6
RET
FS6:
MOV A,R4
RRC A
MOV R4,A
MOV A,R5
RRC A
MOV R5,A
INC R7
RET
FS7:
MOV A,R2
JNZ FS4
CJNE R3,#0,FS10
MOV R6,#41H
FS8:
RET
FS9:
JB ACC.7,FS8
FS10:
MOV C,PWS.5
MOV A,R3
RLC A
MOV R3,A
MOV A,R2
RLC A
MOV R2,A
CLR PWS.5
DEC R6
SJMP FS7
SJMP FA1
FA8:
MOV A,R3
CLR C
SUBB A,R5
MOV R3,A
MOV A,R2
SUBB A,R4
MOV R2,A
JNC FA9
CLR A
CLR C
SUBB A,R3
MOV R3,A
CLR A
SUBB A,R2
MOV R2,A
CPL 38H
SETB C
SJMP FA3
MOV R1,#MIDDLE
LCALL FSTR
MOV P20,#COEFKD
LCALL FMUL
MOV R1,#MIDDLE
LCALL FSTR
MOV R0,#BIASPI
LCALL FADD
MOV R1,#MIDDLE
LCALL FSTR
MOV R0, #BIASPP
LCALL FADD
MOV R1,#BIPID
LCALL FSTR
MOV R0,#BIPID
LCALL FINT
MOV 4FH,4CH
MOV 50H,4DH
MOV 51H,4EH
RET
FSTR:
MOV R4,A
MOV @R1,A
INC R1
MOV A,R2
MOV @R1,A
INC R1
MOV A,R3
MOV @R1,A
DEC R1
DEC R1
RET
FADD:
CLR 3AH
MOV A,@R0
MOV R6,A
INC R0
MOV A,@R0
MOV R2,A
INC R0
MOV A,@R0
MOV R3,A
DEC R0
DEC R0
MOV A,@R1
MOV R7,A
INC R1
MOV A,@R1
MOV R4,A
INC R1
MOV A,@R1
MOV R5,A
DEC R1
DEC R1
FS1:
MOV A,R4
RRC A
MOV R4,A
MOV A,R5
RRC A
MOV R5,A
INC R7
RET
FS2:
MOV A,R2
JNZ FS4
CJNE R3,#0,FS5
MOV R6,#41H
FS3:
RET
FS4:
JB ACC.7,FS3
FS5:
MOV C,PWS.5
MOV A,R3
RLC A
MOV R3,A
MOV A,R2
RLC A
MOV R2,A
CLR PWS.5
DEC R6
SJMP FS2
FA4:
CJNE R2,#0,.FAA
CJNE R3,#0,FAA
MOV R4,#41H
RET
FAA:
MOV A,R6
MOV C,38H
MOV ACC.7,C
XCH A,R4
MOV R6,A
RET
FA5:
CJNE R2,#0,.FA7
CJNE R3,#0,FA7
MOV A,R7
MOV R6,A
SJMP FA2
FA6:
CPL PWS.5
FA7:
CLR C
JC FS2
MOV C,39H
JB PWS.5,FS6
MOV A,R2
RRC A
MOV R2,A
MOV A,R3
RRC A
MOV R3,A
INC R6
RET
FS6:
MOV A,R4
RRC A
MOV R4,A
MOV A,R5
RRC A
MOV R5,A
INC R7
RET
FS7:
MOV A,R2
JNZ FS4
CJNE R3,#0,FS10
MOV R6,#41H
FS8:
RET
FS9:
JB ACC.7,FS8
FS10:
MOV C,PWS.5
MOV A,R3
RLC A
MOV R3,A
MOV A,R2
RLC A
MOV R2,A
CLR PWS.5
DEC R6
SJMP FS7
SJMP FA1
FA8:
MOV A,R3
CLR C
SUBB A,R5
MOV R3,A
MOV A,R2
SUBB A,R4
MOV R2,A
JNC FA9
CLR A
CLR C
SUBB A,R3
MOV R3,A
CLR A
SUBB A,R2
MOV R2,A
CPL 38H
SETB C
SJMP FA3
RET
FINT:
CLR PWS.5
INC
MOV A,@R0
MOV R2,A
INC R0
MOV A,@R0
MOV R3,A
DEC R0
DEC R0
MOV A,@R0
MOV C,ACC.7
MOV 3CH,C
CLR ACC.7
JNB ACC.6,FINT1
MOV R2,#0
MOV R3,#0
RET
FINT1:
CJNE A,#17,$+3
JC FINT3
SETB PWS.5
MOV R2,#0FFH
MOV R3,#0FFH
RET
FINT3:
CJNE A,#16,FINT4
RET
FINT4:
CLR C
XCH A,R2
XCH A,R3
RRC A
XCH A,R3
INC A
SJMP FINT3
RET
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -