1、积分分离 PID控制算法程序2007-01-05 15:23;*P10,积分分离 PID控制算法程序 *;T、TD、TI、KP 依次从 30H，33H，36H，39H 开始。;A，B，C,A,的值依次存在 BLOCK1，BLOCK2，BLOCK3 BLOCK4 的地址里;这里 B与 C值相同; 这里 R(k)给的是定值ORG 0000HE EQU 20H ; 一阈值GK EQU 23H ;G(k)GK1 EQU 26H ;G(k-1)FK EQU 29H ;F(k)FK1 EQU 2CH ;F(k-1)BLOCK1 EQU 50H ;ABLOCK2 EQU 53H ;BBLOCK3 EQU 5
2、6H ;CBLOCK4 EQU 5AH ;AUK EQU 5DH ;存放结果BUFF EQU 43H ;暂存区BUFF1 EQU 46HBUFF2 EQU 49HBUFFR0 EQU 4CHREC EQU 63H ;采样次数RK EQU 66H ;R(k)CK EQU 69H ;采样数据始址EK EQU 6CH ;存放偏差值 E(k)的始址EK1 EQU 70H ;存放 E(k-1)的始址TEST: MOV RK,#01H ;常数 Rk 1.25的 BCD码浮点数MOV RK+1,#12HMOV RK+2,#50H; MOV E,#7EH ;设定一阈值 0.001的 BCD码浮点数; MOV
3、E+1,#10H; MOV E+2,#00HMOV E,#00H ;阈值为 0.3MOV E+1,#30HMOV E+2,#00HMOV 3CH,#01H ;常数 1的 BCD码浮点数MOV 3DH,#10HMOV 3EH,#00HMOV 40H,#01H ;常数 2的 BCD码浮点数MOV 41H,#20HMOV 42H,#00HMOV 30H,#01H ;T 2.34的 BCD 码浮点数MOV 31H,#23HMOV 32H,#40HMOV 33H,#01H ;Td 3.54的 BCD码浮点数MOV 34H,#35HMOV 35H,#40HMOV 36H,#01H ;Ti 1.12的 BC
4、D码浮点数MOV 37H,#11HMOV 38H,#20HMOV 39H,#01H ;Kp 1.25的 BCD码浮点数MOV 3AH,#12HMOV 3BH,#50HMOV R0,#E ; 将其转换成二进制浮点操作数LCALL BTOFMOV R0,#RKLCALL BTOFMOV R0,#3CHLCALL BTOFMOV R0,#40HLCALL BTOFMOV R0,#39HLCALL BTOFMOV R0,#36HLCALL BTOF ;将其转换成二进制浮点操作数MOV R0,#33H ;指向码浮点操作数 TdLCALL BTOF ;将其转换成二进制浮点操作数MOV R0,#30H ;指
5、向码浮点操作数 TLCALL BTOF ;将其转换成二进制浮点操作数MOV R1, #BUFFR0 ;保存 30H中的值 即 T值LCALL FMOVR0MOV R1, #36H ;计算 A值(1+T/Ti+Td/T).KpLCALL FDIVMOV R1,#3CH ;常数 1LCALL FADDMOV R0,#33H ;保存 33H中的值MOV R1,#BUFFLCALL FMOVR0MOV R1,#BUFFR0LCALL FDIVMOV R1,#30H ;30H里存的是 T/Ti+1LCALL FADDMOV R1,#39HLCALL FMULMOV R1 ,#BLOCK1 ;将结果保存在
6、 BLOCK1中LCALL FMOVR0MOV R1,#BUFFR0 ;30H恢复原值MOV R0,#30HLCALL FMOVMOV R1,#BUFF ;33H恢复原值MOV R0,#33HLCALL FMOVMOV R0,#40H ;计算 B的值 Kp.(1+2.Td/T)MOV R1,#33HLCALL FMULMOV R1,#30HLCALL FDIVMOV R1,#3CHLCALL FADDMOV R1,#39HLCALL FMULMOV R1,#BLOCK2 ;保存 B值到 BLOCK2中LCALL FMOVR0MOV R0,#39H ;计算 C的值 Kp.Td/TMOV R1,#
7、33HLCALL FMULMOV R1,#30HLCALL FDIVMOV R1,#BLOCK3 ;保存 C值到 BLOCK3中LCALL FMOVR0MOV R0,#33H ;计算 A,保存 33H中的值MOV R1,#BUFFR0LCALL FMOVR0MOV R1,#30HLCALL FDIVMOV R1,#3CHLCALL FADDMOV R1,#39HLCALL FMULMOV R1,#BLOCK4 ;保存 A值到 BLOCK4中LCALL FMOVR0MOV R1,#BUFFR0 ;恢复 33H中的值LCALL FMOVMOV R0,#EK1 ;将 EK1，FK1,GK1 设初值
8、0LCALL FCLRMOV R0,#FK1LCALL FCLRMOV R0,#GK1LCALL FCLRMOV REC,#04H ;设置采样次数LOOP: MOV CK,#01H ;采样数据暂时给了一个定值MOV CK+1,#10HMOV CK+2,#00HMOV R0,#CKLCALL BTOFMOV R0,#RK ;保存 R（k）中的值MOV R1,#BUFFR0LCALL FMOVR0MOV R1,#CKLCALL FSUB ;计算 R(k)-C(k)的值送给 E（k）MOV R1,#EKLCALL FMOVR0MOV R1,#BUFFR0LCALL FMOV ;恢复 R（K）的值，释
9、放、BUFFR0MOV R0,#BUFFR0 ;EK存到 BUFFR0MOV R1,#EKLCALL FMOVLCALL FABS ;求 EK的绝对值MOV R1,#E ;判断 EK的绝对值与 E的大小LCALL FCMPJC PID ;EKG(k-1)MOV R1,#GKLCALL FMOVMOV R0,#FK1 ;F(k)F(K-1)MOV R1,#FKLCALL FMOVMOV R0,#EK1 ;E(K)E(K-1)MOV R1,#EKLCALL FMOVMOV R0,#UK ;UK转换成 BCD码浮点数输出LCALL FTOBLCALL DELAY ;等待采样时刻DJNZ REC,NE
10、XT1SJMP $NEXT1: LJMP LOOPDELAY: MOV R7,#02HDELAY1: MOV R6,#0FFHDELAY2: DJNZ R6,DELAY2DJNZ R7,DELAY1RET; （） 标号： 功能：浮点数格式化;入口条件：待格式化浮点操作数在R0中。;出口信息：已格式化浮点操作数仍在R0中。;影响资源：PSW、A、R2、R3、R4、位 1FH 堆栈需求： 字节FSDT: LCALL MVR0 ;将待格式化操作数传送到第一工作区中LCALL RLN ;通过左规完成格式化LJMP MOV0 ;将已格式化浮点操作数传回到R0中; （） 标号： 功能：浮点数加法;入口条件
11、：被加数在R0中，加数在R1中。;出口信息：OV=0 时，和仍在R0中，OV=1 时，溢出。;影响资源：PSW、A、B、R2R7、位 1EH、1FH 堆栈需求： 字节FADD: CLR F0 ;设立加法标志SJMP AS ;计算代数和; （） 标号： 功能：浮点数减法;入口条件：被减数在R0中，减数在R1中。;出口信息：OV=0 时，差仍在R0中，OV=1 时，溢出。;影响资源：PSW、A、B、R2R7、位 1EH、1FH 堆栈需求：字节FSUB: SETB F0 ;设立减法标志AS: LCALL MVR1 ;计算代数和。先将R1传送到第二工作区MOV C,F0 ;用加减标志来校正第二操作数的
12、有效符号CLR A ; *？应加的一条语句RRC AXRL A,R1MOV C,ACC.7ASN: MOV 1EH,C ;将第二操作数的有效符号存入位 1EH中XRL A,R0 ;与第一操作数的符号比较RLC AMOV F0,C ;保存比较结果LCALL MVR0 ;将R0传送到第一工作区中LCALL AS1 ;在工作寄存器中完成代数运算MOV0: INC R0 ;将结果传回到R0中的子程序入口INC R0MOV A,R4 ;传回尾数的低字节MOV R0,ADEC R0MOV A,R3 ;传回尾数的高字节MOV R0,ADEC R0MOV A,R2 ;取结果的阶码MOV C,1FH ;取结果的
13、数符MOV ACC.7,C ;拼入阶码中MOV R0,ACLR ACC.7 ;不考虑数符CLR OV ;清除溢出标志CJNE A,#3FH,MV01;阶码是否上溢？ * 应为#40HSETB OV ;设立溢出标志MV01: MOV A,R0 ;取出带数符的阶码RETMVR0: MOV A,R0 ;将R0传送到第一工作区中的子程序MOV C,ACC.7 ;将数符保存在位 1FH中MOV 1FH,CMOV C,ACC.6 ;将阶码扩充为补码MOV ACC.7,CMOV R2,A ;存放在 R2中INC R0MOV A,R0 ;将尾数高字节存放在 R3中MOV R3,A ;INC R0MOV A,R
14、0 ;将尾数低字节存放在 R4中MOV R4,ADEC R0 ;恢复数据指针DEC R0RETMVR1: MOV A,R1 ;将R1传送到第二工作区中的子程序MOV C,ACC.7 ;将数符保存在位 1EH中MOV 1EH,CMOV C,ACC.6 ;将阶码扩充为补码MOV ACC.7,CMOV R5,A ;存放在 R5中INC R1MOV A,R1 ;将尾数高字节存放在 R6中MOV R6,AINC R1MOV A,R1 ;将尾数低字节存放在 R7中MOV R7,ADEC R1 ;恢复数据指针DEC R1RETAS1: MOV A,R6 ;读取第二操作数尾数高字节ORL A,R7JZ AS2
15、 ;第二操作数为零，不必运算MOV A,R3 ;读取第一操作数尾数高字节ORL A,R4JNZ EQMOV A,R6 ;第一操作数为零，结果以第二操作数为准MOV R3,AMOV A,R7MOV R4,AMOV A,R5MOV R2,AMOV C,1EHMOV 1FH,CAS2: RETEQ: MOV A,R2 ;对阶，比较两个操作数的阶码XRL A,R5JZ AS4 ;阶码相同，对阶结束JB ACC.7,EQ3;阶符互异MOV A,R2 ;阶符相同，比较大小CLR CSUBB A,R5JC EQ4EQ2: CLR C ;第二操作数右规一次MOV A,R6 ;尾数缩小一半RRC AMOV R6
16、,AMOV A,R7RRC AMOV R7,AINC R5 ;阶码加一ORL A,R6 ;尾数为零否？JNZ EQ ;尾数不为零，继续对阶MOV A,R2 ;尾数为零，提前结束对阶MOV R5,ASJMP AS4EQ3: MOV A,R2 ;判断第一操作数阶符JNB ACC.7,EQ2;如为正，右规第二操作数EQ4: CLR CLCALL RR1 ;第一操作数右规一次ORL A,R3 ;尾数为零否？JNZ EQ ;不为零，继续对阶MOV A,R5 ;尾数为零，提前结束对阶MOV R2,AAS4: JB F0,AS5 ;尾数加减判断MOV A,R4 ;尾数相加ADD A,R7MOV R4,AMO
17、V A,R3ADDC A,R6MOV R3,AJNC AS2LJMP RR1 ;有进位，右规一次AS5: CLR C ;比较绝对值大小MOV A,R4SUBB A,R7MOV B,AMOV A,R3SUBB A,R6JC AS6MOV R4,B ;第一尾数减第二尾数MOV R3,ALJMP RLN ;结果规格化AS6: CPL 1FH ;结果的符号与第一操作数相反CLR C ;结果的绝对值为第二尾数减第一尾数MOV A,R7SUBB A,R4MOV R4,AMOV A,R6SUBB A,R3MOV R3,ARLN: MOV A,R3 ;浮点数规格化ORL A,R4 ;尾数为零否？JNZ RLN
18、1MOV R2,#0C1H;阶码取最小值 *？应为#C0HRETRLN1: MOV A,R3JB ACC.7,RLN2;尾数最高位为一否？CLR C ;不为一，左规一次LCALL RL1SJMP RLN ;继续判断RLN2: CLR OV ;规格化结束RETRL1: MOV A,R4 ;第一操作数左规一次RLC A ;尾数扩大一倍MOV R4,AMOV A,R3RLC AMOV R3,ADEC R2 ;阶码减一CJNE R2,#0C0H,RL1E;阶码下溢否？ * 应改为 CJNE R2，#0BFH，RL1E；CLR AMOV R3,A ;阶码下溢，操作数以零计MOV R4,AMOV R2,#0C1H ; *应改为 MOV R2，#0C0HRL1E: CLR OVRETRR1: MOV A,R3 ;第一操作数右规一次RRC A ;尾数缩小一半MOV R3,AMOV A,R4RRC A
