14.2.1位置式pid控制.txt

《MCS-51单片机应用开发实用子程序》配套源代码下载

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