精品PID控制算法程序.docx
《精品PID控制算法程序.docx》由会员分享，可在线阅读，更多相关《精品PID控制算法程序.docx（23页珍藏版）》请在冰豆网上搜索。
精品PID控制算法程序
[精品]PID控制算法程序
;********增量式PID控制算法程序***********;T、TD、TI、KP依次从30H，33H，36H，39H开始。
;A，B，C的值依次存在BLOCK1，BLOCK2，BLOCK3的地址里
;这里R（k）给的是定值
;
ORG0000H
BLOCK1EQU43H;A,B,C
BLOCK2EQU46H
BLOCK3EQU49H
UKEQU4CH;存结果UK
RKEQU50H
EKEQU53H;存放偏差值E（k）的始址EK1EQU56H;存放E（k-1）的始址EK2EQU59H;存放E（k-2）的始址CKEQU5CH;采样数据始址BUFFEQU60H;暂存区
BUFF1EQU63H
BUFF2EQU66H
RECEQU69H
TEST:
MOVRK,#01H;常数Rk的BCD码浮点数MOVRK+1,#12H;1.25
MOVRK+2,#50H
MOV3CH,#01H;常数1的BCD码浮点数MOV3DH,#10H
MOV3EH,#00H
MOV40H,#01H;常数2的BCD码浮点数MOV41H,#20H
MOV42H,#00H
MOV30H,#01H;T的BCD码浮点数MOV31H,#23H;2.34
MOV32H,#40H
MOV33H,#01H;Td的BCD码浮点数MOV34H,#35H;3.54
MOV35H,#40H
MOV36H,#01H;Ti的BCD码浮点数MOV37H,#11H;1.12
MOV38H,#20H
MOV39H,#01H;Kp的BCD码浮点数MOV3AH,#12H;1.25
MOV3BH,#50H
MOVR0,#RK;指向BCD码浮点操作数LCALLBTOF;将其转换成二进制浮点操作数MOVR0,#3CH
LCALLBTOF
MOVR0,#40H
LCALLBTOF
MOVR0,#39H
LCALLBTOF
MOVR0,#36H;指向BCD码浮点操作数TiLCALLBTOF;将其转换成二进制浮点操作数MOVR0,#33H;指向BCD码浮点操作数TdLCALLBTOF;将其转换成二进制浮点操作数MOVR0,#30H;指向BCD码浮点操作数TLCALLBTOF;将其转换成二进制浮点操作数
MOVR1,#BUFF1;保存30H中的值即T值LCALLFMOVR0
MOVR1,#36H;计算A值（1+T/Ti+Td/T）.KpLCALLFDIV
MOVR1,#3CH;常数1
LCALLFADD
MOVR0,#33H;保存33H中的值MOVR1,#BUFF
LCALLFMOVR0
MOVR1,#BUFF1
LCALLFDIV
MOVR1,#30H;30H里存的是T/Ti+1LCALLFADD
MOVR1,#39H
LCALLFMUL
MOVR1,#BLOCK1;将结果保存在BLOCK1中
LCALLFMOVR0
MOVR1,#BUFF1;30H恢复原值MOVR0,#30H
LCALLFMOV
MOVR1,#BUFF;33H恢复原值MOVR0,#33H
LCALLFMOV
MOVR0,#40H;计算B的值Kp.（1+2.Td/T）MOVR1,#33H
LCALLFMUL
MOVR1,#30H
LCALLFDIV
MOVR1,#3CH
LCALLFADD
MOVR1,#39H
LCALLFMUL
MOVR1,#BLOCK2;保存B值到BLOCK2中LCALLFMOVR0
MOVR0,#39H;计算C的值Kp.Td/TMOVR1,#33H
LCALLFMUL
MOVR1,#30H
LCALLFDIV
MOVR1,#BLOCK3;保存C值到BLOCK3中LCALLFMOVR0
MOVR0,#EK1;将EK1，EK2设初值0
LCALLFCLR
MOVR0,#EK2
LCALLFCLR
MOVREC,#03H;设置采样次数LOOP:
MOVCK,#7eH;采样数据暂时给了一个定值
MOVCK+1,#21H;0.002112MOVCK+2,#12H
MOVR0,#CK
LCALLBTOF
MOVR0,#RK;保存R（k）中的值MOVR1,#BUFF
LCALLFMOVR0
MOVR1,#CK
LCALLFSUB;计算R（k）-C（k）的值送给E（k）
MOVR1,#EK
LCALLFMOVR0
MOVR1,#BUFF;恢复RK的值释放BUFFMOVR0,#RK
LCALLFMOV
MOVR0,#BLOCK2;将B.e（k-1）的值暂存在BUFF1中
MOVR1,#BUFF;保存BLCALLFMOVR0
MOVR1,#EK1
LCALLFMUL
MOVR1,#BUFF1
LCALLFMOVR0
MOVR1,#BUFF;恢复B释放BUFFLCALLFMOV
MOVR0,#BLOCK3;将C.e（K-2）的值暂存在BUFF2中
MOVR1,#BUFF;保存C
LCALLFMOVR0
MOVR1,#EK2
LCALLFMUL
MOVR1,#BUFF2
LCALLFMOVR0
MOVR1,#BUFF;恢复C释放BUFFLCALLFMOV
MOVR0,#BLOCK1;A.E（k）MOVR1,#BUFF
LCALLFMOVR0
MOVR1,#EK
LCALLFMUL
MOVR1,#BUFF1;计算Uk值A.E（k）-B.E（k-1）+C.E（k-2）
LCALLFSUB
MOVR1,#BUFF2
LCALLFADD
MOVR1,#UK;保存结果到UK中LCALLFMOVR0
MOVR1,#BUFF;恢复A释放BUFFLCALLFMOV
MOVR0,#UK;UK转换成BCD码浮点数输出LCALLFTOB
MOVR1,#EK1;将E（k-1）-->E（k-2）,E（k）-->E（k-1）MOVR0,#EK2
LCALLFMOV
MOVR1,#EK
MOVR0,#EK1
LCALLFMOV
LCALLDELAY;等待采样时刻
DJNZREC,NEXT1
SJMP$
NEXT1:
LJMPLOOP
DELAY:
MOVR7,#02H
DELAY1:
MOVR6,#0FFH
DELAY2:
DJNZR6,DELAY2
DJNZR7,DELAY1
RET
;（,）标号:
FSDT功能:
浮点数格式化
;入口条件:
待格式化浮点操作数在[R0]中。
;出口信息:
已格式化浮点操作数仍在[R0]中。
;影响资源:
PSW、A、R2、R3、R4、位1FH堆栈需求:
字节
FSDT:
LCALLMVR0;将待格式化操作数传送到第一工作区中
LCALLRLN;通过左规完成格式化
LJMPMOV0;将已格式化浮点操作数传回到[R0]中
;（,）标号:
FADD功能:
浮点数加法
;入口条件:
被加数在[R0]中，加数在[R1]中。
;出口信息:
OV=0时，和仍在[R0]中，OV=1时，溢出。
;;影响资源:
PSW、A、B、R2,R7、位1EH、1FH堆栈需求:
字节
FADD:
CLRF0;设立加法标志
SJMPAS;计算代数和
;（,）标号:
FSUB功能:
浮点数减法
;入口条件:
被减数在[R0]中，减数在[R1]中。
;出口信息:
OV=0时，差仍在[R0]中，OV=1时，溢出。
;影响资源:
PSW、A、B、R2,R7、位1EH、1FH堆栈需求:
字节
FSUB:
SETBF0;设立减法标志
AS:
LCALLMVR1;计算代数和。
先将[R1]传送到第二工作区
MOVC,F0;用加减标志来校正第二操作数的有效符号
CLRA;********,,,应加的一条语句RRCA
XRLA,@R1
MOVC,ACC.7
ASN:
MOV1EH,C;将第二操作数的有效符号存入位1EH中
XRLA,@R0;与第一操作数的符号比较RLCA
MOVF0,C;保存比较结果
LCALLMVR0;将[R0]传送到第一工作区中LCALLAS1;在工作寄存器中完成代数运算MOV0:
INCR0;将结果传回到[R0]中的子程序入口INCR0
MOVA,R4;传回尾数的低字节MOV@R0,A
DECR0
MOVA,R3;传回尾数的高字节MOV@R0,A
DECR0
MOVA,R2;取结果的阶码
MOVC,1FH;取结果的数符
MOVACC.7,C;拼入阶码中
MOV@R0,A
CLRACC.7;不考虑数符
CLROV;清除溢出标志
CJNEA,#3FH,MV01;阶码是否上溢,********应为#40H
SETBOV;设立溢出标志
MV01:
MOVA,@R0;取出带数符的阶码RET
MVR0:
MOVA,@R0;将[R0]传送到第一工作区中的子程序
MOVC,ACC.7;将数符保存在位1FH中MOV1FH,C
MOVC,ACC.6;将阶码扩充为8bit补码MOVACC.7,C
MOVR2,A;存放在R2中
INCR0
MOVA,@R0;将尾数高字节存放在R3中MOVR3,A;
INCR0
MOVA,@R0;将尾数低字节存放在R4中MOVR4,A
DECR0;恢复数据指针
DECR0
RET
MVR1:
MOVA,@R1;将[R1]传送到第二工作区中的子程序
MOVC,ACC.7;将数符保存在位1EH中MOV1EH,C
MOVC,ACC.6;将阶码扩充为8bit补码MOVACC.7,C
MOVR5,A;存放在R5中
INCR1
MOVA,@R1;将尾数高字节存放在R6中MOVR6,A
INCR1
MOVA,@R1;将尾数低字节存放在R7中MOVR7,A
DECR1;恢复数据指针
DECR1
RET
AS1:
MOVA,R6;读取第二操作数尾数高字节ORLA,R7
JZAS2;第二操作数为零，不必运算MOVA,R3;读取第一操作数尾数高字节ORLA,R4
JNZEQ
MOVA,R6;第一操作数为零，结果以第二操作数为准
MOVR3,A
MOVA,R7
MOVR4,A
MOVA,R5
MOVR2,A
MOVC,1EH
MOV1FH,C
AS2:
RET
EQ:
MOVA,R2;对阶，比较两个操作数的阶码XRLA,R5
JZAS4;阶码相同，对阶结束JBACC.7,EQ3;阶符互异MOVA,R2;阶符相同，比较大小CLRC
SUBBA,R5
JCEQ4
EQ2:
CLRC;第二操作数右规一次MOVA,R6;尾数缩小一半RRCA
MOVR6,A
MOVA,R7
RRCA
MOVR7,A
INCR5;阶码加一
ORLA,R6;尾数为零否,JNZEQ;尾数不为零，继续对阶
MOVA,R2;尾数为零，提前结束对阶MOVR5,A
SJMPAS4
EQ3:
MOVA,R2;判断第一操作数阶符JNBACC.7,EQ2;如为正，右规第二操作数EQ4:
CLRC
LCALLRR1;第一操作数右规一次ORLA,R3;尾数为零否,JNZEQ;不为零，继续对阶
MOVA,R5;尾数为零，提前结束对阶MOVR2,A
AS4:
JBF0,AS5;尾数加减判断MOVA,R4;尾数相加ADDA,R7
MOVR4,A
MOVA,R3
ADDCA,R6
MOVR3,A
JNCAS2
LJMPRR1;有进位，右规一次
AS5:
CLRC;比较绝对值大小MOVA,R4
SUBBA,R7
MOVB,A
MOVA,R3
SUBBA,R6
JCAS6
MOVR4,B;第一尾数减第二尾数MOVR3,A
LJMPRLN;结果规格化AS6:
CPL1FH;结果的符号与第一操作数相反
CLRC;结果的绝对值为第二尾数减第一尾数
MOVA,R7
SUBBA,R4
MOVR4,A
MOVA,R6
SUBBA,R3
MOVR3,A
RLN:
MOVA,R3;浮点数规格化ORLA,R4;尾数为零否,JNZRLN1
MOVR2,#0C0H;阶码取最小值RET
RLN1:
MOVA,R3
JBACC.7,RLN2;尾数最高位为一否,CLRC;不为一，左规一次LCALLRL1
SJMPRLN;继续判断RLN2:
CLROV;规格化结束RET
RL1:
MOVA,R4;第一操作数左规一次RLCA;尾数扩大一倍MOVR4,A
MOVA,R3
RLCA
MOVR3,A
DECR2;阶码减一
CJNER2,#0BFH,RL1E;阶码下溢否,;CLRA
MOVR3,A;阶码下溢，操作数以零计
MOVR4,A
MOVR2,#0C0H;
RL1E:
CLROV
RET
RR1:
MOVA,R3;第一操作数右规一次RRCA;尾数缩小一半
MOVR3,A
MOVA,R4
RRCA
MOVR4,A
INCR2;阶码加一
CLROV;清溢出标志
CJNER2,#40H,RR1E;阶码上溢否,MOVR2,#3FH;阶码溢出
SETBOV
RR1E:
RET
;（4）标号:
FMUL功能:
浮点数乘法
;入口条件:
被乘数在[R0]中，乘数在[R1]中。
;出口信息:
OV=0时，积仍在[R0]中，OV=1时，溢出。
;影响资源:
PSW、A、B、R2,R7、位1EH、1FH堆栈需求:
6字节
FMUL:
LCALLMVR0;将[R0]传送到第一工作区中
MOVA,@R0
XRLA,@R1;比较两个操作数的符号
RLCA
MOV1FH,C;保存积的符号LCALLMUL0;计算积的绝对值LJMPMOV0;将结果传回到[R0]中MUL0:
LCALLMVR1;将[R1]传送到第二工作区中
MUL1:
MOVA,R3;第一尾数为零否,ORLA,R4
JZMUL6
MOVA,R6;第二尾数为零否,ORLA,R7
JZMUL5
MOVA,R7;计算R3R4×R6R7,?
R3R4MOVB,R4
MULAB
MOVA,B
XCHA,R7
MOVB,R3
MULAB
ADDA,R7
MOVR7,A
CLRA
ADDCA,B
XCHA,R4
MOVB,R6MULABADDA,R7MOVR7,AMOVA,BADDCA,R4MOVR4,ACLRARLCAXCHA,R3MOVB,R6MULABADDA,R4MOVR4,AMOVA,BADDCA,R3MOVR3,AJBACC.7,MUL2;积为规格化数否,R7四舍五入
MOVA,R7;左规一次
RLCAMOVR7,ALCALLRL1MUL2:
MOVA,R7JNBACC.7,MUL3INCR4MOVA,R4
JNZMUL3
INCR3
MOVA,R3
JNZMUL3
MOVR3,#80H
INCR2
MUL3:
MOVA,R2;求积的阶码ADDA,R5
MD:
MOVR2,A;阶码溢出判断
JBACC.7,MUL4
JNBACC.6,MUL6
MOVR2,#3FH;阶码上溢，设立标志SETBOV
RET
MUL4:
JBACC.6,MUL6MUL5:
CLRA;结果清零（因子为零或阶码下溢）
MOVR3,A
MOVR4,A
MOVR2,#41H
MUL6:
CLROV
RET
;（5）标号:
FDIV功能:
浮点数除法
;入口条件:
被除数在[R0]中，除数在[R1]中。
;出口信息:
OV=0时，商仍在[R0]中，OV=1时，溢出。
;影响资源:
PSW、A、B、R2,R7、位1EH、1FH堆栈需求:
字节
FDIV:
INCR0
MOVA,@R0
INCR0
ORLA,@R0
DECR0
DECR0
JNZDIV1
MOV@R0,#41H;被除数为零，不必运算CLROV
RET
DIV1:
INCR1
MOVA,@R1
INCR1
ORLA,@R1
DECR1
DECR1
JNZDIV2
SETBOV;除数为零，溢出RET
DIV2:
LCALLMVR0;将[R0]传送到第一工作区中
MOVA,@R0
XRLA,@R1;比较两个操作数的符号RLCA
MOV1FH,C;保存结果的符号LCALLMVR1;将[R1]传送到第二工作区中LCALLDIV3;调用工作区浮点除法LJMPMOV0;回传结果
DIV3:
CLRC;比较尾数的大小MOVA,R4
SUBBA,R7
MOVA,R3
SUBBA,R6
JCDIV4
LCALLRR1;被除数右规一次SJMPDIV3
DIV4:
CLRA;借用R0R1R2作工作寄存器XCHA,R0;清零并保护之PUSHACC
CLRA
XCHA,R1
PUSHACC
MOVA,R2
PUSHACC
MOVB,#10H;除法运算，R3R4,R6R7,?
R0R1
DIV5:
CLRC
MOVA,R1
RLCAMOVR1,AMOVA,R0RLCAMOVR0,AMOVA,R4RLCAMOVR4,AXCHA,R3RLCAXCHA,R3MOVF0,CCLRC
SUBBA,R7MOVR2,AMOVA,R3SUBBA,R6ANLC,/F0JCDIV6MOVR3,AMOVA,R2MOVR4,AINCR1DIV6:
DJNZB,DIV5
MOVA,R6;四舍五入
CLRC
RRCA
SUBBA,R3
CLRA
ADDCA,R1;将结果存回R3R4MOVR4,A
CLRA
ADDCA,R0
MOVR3,A
POPACC;恢复R0R1R2MOVR2,A
POPACC
MOVR1,A
POPACC
MOVR0,A
MOVA,R2;计算商的阶码CLRC
SUBBA,R5
LCALLMD;阶码检验LJMPRLN;规格化
;（6）标号:
FCLR功能:
浮点数清零
;入口条件:
操作数在[R0]中。
;出口信息:
操作数被清零。
;影响资源:
A堆栈需求:
字节
FCLR:
INCR0
INCR0
CLRA
MOV@R0,A
DECR0
MOV@R0,A
DECR0
MOV@R0,#41H
RET
;（7）标号:
FZER功能:
浮点数判零
;入口条件:
操作数在[R0]中。
;出口信息:
若累加器A为零，则操作数[R0]为零，否则不为零。
;影响资源:
A堆栈需求:
字节
FZER:
INCR0
INCR0
MOVA,@R0
DECR0
ORLA,@R0
DECR0
JNZZERO
MOV@R0,#41H
ZERO:
RET
;（8）标号:
FMOV功能:
浮点数传送
;入口条件:
源操作数在[R1]中，目标地址为[R0]。
;出口信息:
[R0]=[R1]，[R1]不变。
;影响资源:
A堆栈需求:
字节
FMOV:
INCR0
INCR0
INCR1
INCR1
MOVA,@R1
MOV@R0,A
DECR0
DECR1
MOVA,@R1
MOV@R0,A
DECR0
DECR1
MOVA,@R1
MOV@R0,A
RET
;（8.1）标号:
FMOVR0功能:
浮点数传送
;入口条件:
源操作数在[R0]中，目标地址为[R1]。
;出口信息:
[R1]=[R0]，[R0]不变。
;影响资源:
A堆栈需求:
字节
FMOVR0:
INCR1
INCR1
INCR0
INCR0
MOVA,@R0
MOV@R1,A
DECR1
DECR0
MOVA,@R0
MOV@R1,A
DECR1
DECR0
MOVA,@R0
MOV@R1,A
RET
;（24）标号:
DTOF功能:
双字节十六进制定点数转换成格式化浮点数
;入口条件:
双字节定点数的绝对值在[R0]中，数符在位1FH中，整数部分的位数在A中。
;出口信息:
转换成格式化浮点数在[R0]中（三字节）。
;影响资源:
PSW、A、R2、R3、R4、位1FH堆栈需求:
字节
DTOF:
MOVR2,A;按整数的位数初始化阶码MOVA,@R0;将定点数作尾数
MOVR3,A
INCR0
MOVA,@R0
MOVR4,A
DECR0
LCALLRLN;进行规格化
LJMPMOV0;传送结果到[R0]中
;（25）标号:
FTOD功能:
格式化浮点数转换成双字节定点数
;入口条件:
格式化浮点操作数在[R0]中。
;出口信息:
OV=1时溢出，OV=0时转换成功:
定点数的绝对值在[R0]中（双字节），数
升级会员 