积分分离PID控制算法程序

积分分离PID控制算法程序2007-01-0515:23;**********P10,积分分离PID控制算法程序********;T、TD、TI、KP依次从30H，33H，36H，39H开始。;A，B，C,A',的值依次存在BLOCK1，BLOCK2，BLOCK3BLOCK4的地址里;这里B'与C值相同;这里R(k)给的是定值ORG0000HEEQU20H;一阈值GKEQU23H;G(k)GK1EQU26H;G(k-1)FKEQU29H;F(k)FK1EQU2CH;F(k-1)BLOCK1EQU50H;ABLOCK2EQU53H;BBLOCK3EQU56H;CBLOCK4EQU5AH;A'UKEQU5DH;存放结果BUFFEQU43H;暂存区BUFF1EQU46HBUFF2EQU49HBUFFR0EQU4CHRECEQU63H;采样次数RKEQU66H;R(k)CKEQU69H;采样数据始址EKEQU6CH;存放偏差值E(k)的始址EK1EQU70H;存放E(k-1)的始址TEST:MOVRK,#01H;常数Rk1.25的BCD码浮点数MOVRK+1,#12HMOVRK+2,#50H;MOVE,#7EH;设定一阈值0.001的BCD码浮点数;MOVE+1,#10H;MOVE+2,#00HMOVE,#00H;阈值为0.3MOVE+1,#30HMOVE+2,#00HMOV3CH,#01H;常数1的BCD码浮点数MOV3DH,#10HMOV3EH,#00HMOV40H,#01H;常数2的BCD码浮点数MOV41H,#20HMOV42H,#00HMOV30H,#01H;T2.34的BCD码浮点数MOV31H,#23HMOV32H,#40HMOV33H,#01H;Td3.54的BCD码浮点数MOV34H,#35HMOV35H,#40HMOV36H,#01H;Ti1.12的BCD码浮点数MOV37H,#11HMOV38H,#20HMOV39H,#01H;Kp1.25的BCD码浮点数MOV3AH,#12HMOV3BH,#50HMOVR0,#E;将其转换成二进制浮点操作数LCALLBTOFMOVR0,#RKLCALLBTOFMOVR0,#3CHLCALLBTOFMOVR0,#40HLCALLBTOFMOVR0,#39HLCALLBTOFMOVR0,#36HLCALLBTOF;将其转换成二进制浮点操作数MOVR0,#33H;指向ＢＣＤ码浮点操作数TdLCALLBTOF;将其转换成二进制浮点操作数MOVR0,#30H;指向ＢＣＤ码浮点操作数TLCALLBTOF;将其转换成二进制浮点操作数MOVR1,#BUFFR0;保存30H中的值即T值LCALLFMOVR0MOVR1,#36H;计算A值(1+T/Ti+Td/T).KpLCALLFDIVMOVR1,#3CH;常数1LCALLFADDMOVR0,#33H;保存33H中的值MOVR1,#BUFFLCALLFMOVR0MOVR1,#BUFFR0LCALLFDIVMOVR1,#30H;30H里存的是T/Ti+1LCALLFADDMOVR1,#39HLCALLFMULMOVR1,#BLOCK1;将结果保存在BLOCK1中LCALLFMOVR0MOVR1,#BUFFR0;30H恢复原值MOVR0,#30HLCALLFMOVMOVR1,#BUFF;33H恢复原值MOVR0,#33HLCALLFMOVMOVR0,#40H;计算B的值Kp.(1+2.Td/T)MOVR1,#33HLCALLFMULMOVR1,#30HLCALLFDIVMOVR1,#3CHLCALLFADDMOVR1,#39HLCALLFMULMOVR1,#BLOCK2;保存B值到BLOCK2中LCALLFMOVR0MOVR0,#39H;计算C的值Kp.Td/TMOVR1,#33HLCALLFMULMOVR1,#30HLCALLFDIVMOVR1,#BLOCK3;保存C值到BLOCK3中LCALLFMOVR0MOVR0,#33H;计算A',保存33H中的值MOVR1,#BUFFR0LCALLFMOVR0MOVR1,#30HLCALLFDIVMOVR1,#3CHLCALLFADDMOVR1,#39HLCALLFMULMOVR1,#BLOCK4;保存A'值到BLOCK4中LCALLFMOVR0MOVR1,#BUFFR0;恢复33H中的值LCALLFMOVMOVR0,#EK1;将EK1，FK1,GK1设初值0LCALLFCLRMOVR0,#FK1LCALLFCLRMOVR0,#GK1LCALLFCLRMOVREC,#04H;设置采样次数LOOP:MOVCK,#01H;采样数据暂时给了一个定值MOVCK+1,#10HMOVCK+2,#00HMOVR0,#CKLCALLBTOFMOVR0,#RK;保存R（k）中的值MOVR1,#BUFFR0LCALLFMOVR0MOVR1,#CKLCALLFSUB;计算R(k)-C(k)的值送给E（k）MOVR1,#EKLCALLFMOVR0MOVR1,#BUFFR0LCALLFMOV;恢复R（K）的值，释放、BUFFR0MOVR0,#BUFFR0;EK存到BUFFR0MOVR1,#EKLCALLFMOVLCALLFABS;求EK的绝对值MOVR1,#E;判断EK的绝对值与E的大小LCALLFCMPJCPID;EK=E时转PID控制程序否则PD程序CJNEA,#00H,PDLJMPPIDPD:MOVR0,#BLOCK4MOVR1,#BUFFR0;保存A'的值LCALLFMOVR0MOVR1,#EKLCALLFMULMOVR1,#FK1LCALLFSUBMOVR1,#UK;结果存在UK中LCALLFMOVR0MOVR1,#BUFFR0;恢复A'LCALLFMOVMOVR0,#BLOCK3;计算B'.E(k)这里B'值与C值同MOVR1,#BUFFR0;保存B'LCALLFMOVR0MOVR1,#EKLCALLFMULMOVR1,#FK;B'.EK存到FK中LCALLFMOVR0MOVR1,#BUFFR0;恢复B'LCALLFMOVLJMPOUTPID:MOVR0,#BLOCK1;计算A.E(k)+G(K-1)MOVR1,#BUFFR0;保存ALCALLFMOVR0MOVR1,#EKLCALLFMULMOVR1,#GK1LCALLFADDMOVR1,#UK;计算结果存入UK中LCALLFMOVR0MOVR1,#BUFFR0;恢复A值LCALLFMOVMOVR0,#BLOCK2;计算U(k)-B.E(k)+C.E(k-1)MOVR1,#BUFFR0;保存B到BUFFR0LCALLFMOVR0MOVR1,#EKLCALLFMULMOVR1,#BUFF;B.E(k)保存到BUFFLCALLFMOVR0MOVR0,#UKMOVR1,#BUFF1;UK保存到BUFF1LCALLFMOVR0MOVR1,#BUFFLCALLFSUBMOVR1,#BUFF;U(k)-BE(k)保存到BUFFLCALLFMOVR0MOVR0,#BLOCK3;计算C.E(k-1)MOVR1,#BUFF2;保存C到BUFF2LCALLFMOVR0MOVR1,#EK1LCALLFMULMOVR1,#BUFFLCALLFADDMOVR1,#GK;U（k）-BE(k)+CE(k-1)保存到GKLCALLFMOVR0MOVR1,#BUFF2;恢复C值释放BUFF2LCALLFMOVMOVR0,#UK;恢复UK值释放BUFF1MOVR1,#BUFF1LCALLFMOVMOVR0,#BLOCK2;恢复BMOVR1,#BUFFR0LCALLFMOVOUT:MOVR0,#GK1;G(k)--G(k-1)MOVR1,#GKLCALLFMOVMOVR0,#FK1;F(k)--F(K-1)MOVR1,#FKLCALLFMOVMOVR0,#EK1;E(K)--E(K-1)MOVR1,#EKLCALLFMOVMOVR0,#UK;UK转换成BCD码浮点数输出LCALLFTOBLCALLDELAY;等待采样时刻DJNZREC,NEXT1SJMP$NEXT1:LJMPLOOPDELAY:MOVR7,#02HDELAY1:MOVR6,#0FFHDELAY2:DJNZR6,DELAY2DJNZR7,DELAY1RET;（１）标号：ＦＳＤＴ功能：浮点数格式化;入口条件：待格式化浮点操作数在[R0]中。;出口信息：已格式化浮点操作数仍在[R0]中。;影响资源：PSW、A、R2、R3、R4、位1FH堆栈需求：６字节FSDT:LCALLMVR0;将待格式化操作数传送到第一工作区中LCALLRLN;通过左规完成格式化LJMPMOV0;将已格式化浮点操作数传回到[R0]中;（２）标号：ＦＡＤＤ功能：浮点数加法;入口条件：被加数在[R0]中，加数在[R1]中。;出口信息：OV=0时，和仍在[R0]中，OV=1时，溢出。;;影响资源：PSW、A、B、R2～R7、位1EH、1FH堆栈需求：６字节FADD:CLRF0;设立加法标志SJMPAS;计算代数和;（３）标号：ＦＳＵＢ功能：浮点数减法;入口条件：被减数在[R0]中，减数在[R1]中。;出口信息：OV=0时，差仍在[R0]中，OV=1时，溢出。;影响资源：PSW、A、B、R2～R7、位1EH、1FH堆栈需求：６字节FSUB:SETBF0;设立减法标志AS:LCALLMVR1;计算代数和。先将[R1]传送到第二工作区MOVC,F0;用加减标志来校正第二操作数的有效符号CLRA;********？？？应加的一条语句RRCAXRLA,@R1MOVC,ACC.7ASN:MOV1EH,C;将第二操作数的有效符号存入位1EH中XRLA,@R0;与第一操作数的符号比较RLCAMOVF0,C;保存比较结果LCALLMVR0;将[R0]传送到第一工作区中LCALLAS1;在工作寄存器中完成代数运算MOV0:INCR0;将结果传回到[R0]中的子程序入口INCR0MOVA,R4;传回尾数的低字节MOV@R0,ADECR0MOVA,R3;传回尾数的高字节MOV@R0,ADECR0MOVA,R2;取结果的阶码MOVC,1FH;取结果的数符MOVACC.7,C;拼入阶码中MOV@R0,ACLRACC.7;不考虑数符CLROV;清除溢出标志CJNEA,#3FH,MV01;阶码是否上溢？********应为#40HSETBOV;设立溢出标志MV01:MOVA,@R0;取出带数符的阶码RETMVR0:MOVA,@R0;将[R0]传送到第一工作区中的子程序MOVC,ACC.7;将数符保存在位1FH中MOV1FH,CMOVC,ACC.6;将阶码扩充为８ｂｉｔ补码MOVACC.7,CMOVR2,A;存放在R2中INCR0MOVA,@R0;将尾数高字节存放在R3中MOVR3,A;INCR0MOVA,@R0;将尾数低字节存放在R4中MOVR4,ADECR0;恢复数据指针DECR0RETMVR1:MOVA,@R1;将[R1]传送到第二工作区中的子程序MOVC,ACC.7;将数符保存在位1EH中MOV1EH,CMOVC,ACC.6;将阶码扩充为８ｂｉｔ补码MOVACC.7,CMOVR5,A;存放在R5中INCR1MOVA,@R1;将尾数高字节存放在R6中MOVR6,AINCR1MOVA,@R1;将尾数低字节存放在R7中MOVR7,ADECR1;恢复数据指针DECR1RETAS1:MOVA,R6;读取第二操作数尾数高字节ORLA,R7JZAS2;第二操作数为零，不必运算MOVA,R3;读取第一操作数尾数高字节ORLA,R4JNZEQMOVA,R6;第一操作数为零，结果以第二操作数为准MOVR3,AMOVA,R7MOVR4,AMOVA,R5MOVR2,AMOVC,1EHMOV1FH,CAS2:RETEQ:MOVA,R2;对阶，比较两个操作数的阶码XRLA,R5JZAS4;阶码相同，对阶结束J