增量式PID控制算法程序.docx
- 文档编号:30619666
- 上传时间:2023-08-18
- 格式:DOCX
- 页数:27
- 大小:21.65KB
增量式PID控制算法程序.docx
《增量式PID控制算法程序.docx》由会员分享,可在线阅读,更多相关《增量式PID控制算法程序.docx(27页珍藏版)》请在冰豆网上搜索。
增量式PID控制算法程序
增量式PID控制算法程序 (A51)
点击数:
2146 发布日期:
2006-9-2217:
12:
00
【收藏】 【评论】 【打印】 【编程爱好者论坛】 【关闭】
;********增量式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码浮点操作数Ti
LCALLBTOF;将其转换成二进制浮点操作数
MOVR0,#33H;指向BCD码浮点操作数Td
LCALLBTOF;将其转换成二进制浮点操作数
MOVR0,#30H;指向BCD码浮点操作数T
LCALLBTOF;将其转换成二进制浮点操作数
MOVR1,#BUFF1;保存30H中的值即T值
LCALLFMOVR0
MOVR1,#36H;计算A值(1+T/Ti+Td/T).Kp
LCALLFDIV
MOVR1,#3CH;常数1
LCALLFADD
MOVR0,#33H;保存33H中的值
MOVR1,#BUFF
LCALLFMOVR0
MOVR1,#BUFF1
LCALLFDIV
MOVR1,#30H;30H里存的是T/Ti+1
LCALLFADD
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/T
MOVR1,#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.002112
MOVCK+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的值释放BUFF
MOVR0,#RK
LCALLFMOV
MOVR0,#BLOCK2;将B.e(k-1)的值暂存在BUFF1中
MOVR1,#BUFF;保存B
LCALLFMOVR0
MOVR1,#EK1
LCALLFMUL
MOVR1,#BUFF1
LCALLFMOVR0
MOVR1,#BUFF;恢复B释放BUFF
LCALLFMOV
MOVR0,#BLOCK3;将C.e(K-2)的值暂存在BUFF2中
MOVR1,#BUFF;保存C
LCALLFMOVR0
MOVR1,#EK2
LCALLFMUL
MOVR1,#BUFF2
LCALLFMOVR0
MOVR1,#BUFF;恢复C释放BUFF
LCALLFMOV
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释放BUFF
LCALLFMOV
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
;(1)标号:
FSDT功能:
浮点数格式化
;入口条件:
待格式化浮点操作数在[R0]中。
;出口信息:
已格式化浮点操作数仍在[R0]中。
;影响资源:
PSW、A、R2、R3、R4、位1FH堆栈需求:
6字节
FSDT:
LCALLMVR0;将待格式化操作数传送到第一工作区中
LCALLRLN;通过左规完成格式化
LJMPMOV0;将已格式化浮点操作数传回到[R0]中
;(2)标号:
FADD功能:
浮点数加法
;入口条件:
被加数在[R0]中,加数在[R1]中。
;出口信息:
OV=0时,和仍在[R0]中,OV=1时,溢出。
;;影响资源:
PSW、A、B、R2~R7、位1EH、1FH堆栈需求:
6字节
FADD:
CLRF0;设立加法标志
SJMPAS;计算代数和
;(3)标号:
FSUB功能:
浮点数减法
;入口条件:
被减数在[R0]中,减数在[R1]中。
;出口信息:
OV=0时,差仍在[R0]中,OV=1时,溢出。
;影响资源:
PSW、A、B、R2~R7、位1EH、1FH堆栈需求:
6字节
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,#0C1H;阶码取最小值******?
?
应为#C0H
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,#0C0H,RL1E;阶码下溢否?
*****应改为CJNER2,#0BFH,RL1E;
CLRA
MOVR3,A;阶码下溢,操作数以零计
MOVR4,A
MOVR2,#0C1H;******应改为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-→R3R4
MOVB,R4
MULAB
MOVA,B
XCHA,R7
MOVB,R3
MULAB
ADDA,R7
MOVR7,A
CLRA
ADDCA,B
XCHA,R4
MOVB,R6
MULAB
ADDA,R7
MOVR7,A
MOVA,B
ADDCA,R4
MOVR4,A
CLRA
RLCA
XCHA,R3
MOVB,R6
MULAB
ADDA,R4
MOVR4,A
MOVA,B
ADDCA,R3
MOVR3,A
JBACC.7,MUL2;积为规格化数否?
R7四舍五入
MOVA,R7;左规一次
RLCA
MOVR7,A
LCALLRL1
MUL2:
MOVA,R7
JNBACC.7,MUL3
INCR4
MOVA,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,MUL6
MUL5:
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堆栈需求:
5字节
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
RLCA
MOVR1,A
MOVA,R0
RLCA
MOVR0,A
MOVA,R4
RLCA
MOVR4,A
XCHA,R3
RLCA
XCHA,R3
MOVF0,C
CLRC
SUBBA,R7
MOVR2,A
MOVA,R3
SUBBA,R6
ANLC,/F0
JCDIV6
MOVR3,A
MOVA,R2
MOVR4,A
INCR1
DIV6:
DJNZB,DIV5
MOVA,R6;四舍五入
CLRC
RRCA
SUBBA,R3
CLRA
ADDCA,R1;将结果存回R3R4
MOVR4,A
CLRA
ADDCA,R0
MOVR3,A
POPACC;恢复R0R1R2
MOVR2,A
POPACC
MOVR1,A
POPACC
MOVR0,A
MOVA,R2;计算商的阶码
CLRC
SUBBA,R5
LCALLMD;阶码检验
LJMPRLN;规格化
;(6)标号:
FCLR功能:
浮点数清零
;入口条件:
操作数在[R0]中。
;出口信息:
操作数被清零。
;影响资源:
A堆栈需求:
2字节
FCLR:
INCR0
INCR0
CLRA
MOV@R0,A
DECR0
MOV@R0,A
DECR0
MOV@R0,#41H
RET
;(7)标号:
FZER功能:
浮点数判零
;入口条件:
操作数在[R0]中。
;出口信息:
若累加器A为零,则操作数[R0]为零,否则不为零。
;影响资源:
A堆栈需求:
2字节
FZER:
INCR0
INCR0
MOVA,@R0
DECR0
ORLA,@R0
DECR0
JNZZERO
MOV@R0,#41H
ZERO:
RET
;(8)标号:
FMOV功能:
浮点数传送
;入口条件:
源操作数在[R1]中,目标地址为[R0]。
;出口信息:
[R0]=[R1],[R1]不变。
;影响资源:
A堆栈需求:
2字节
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堆栈需求:
2字节
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堆栈需求:
6字节
DTOF:
MOVR2,A;按整数的位数初始化阶码
MOVA,@R0;将定点数作尾数
MOVR3,A
INCR0
MOVA,@R0
MOVR4,A
DECR0
LCALLRLN;进行规格化
LJMPMOV0;传送结果到[R0]中
;(25)标号:
FTOD功能:
格式化浮点数转换成双字节定点数
;入口条件:
格式化浮点操作数在[R0]中。
;出口信息:
OV=1时溢出,OV=0时转换成功:
定点数的绝对值在[R0]中(
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 增量 PID 控制 算法 程序