积分分离PID控制算法程序.doc

积分分离pid控制算法程序2007-01-05 15: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;aukequ5dh;存放结果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;ekg(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、r2r7、位1eh、1fh堆栈需求：字节fadd:clrf0;设立加法标志sjmpas;计算代数和;（）标号：功能：浮点数减法;入口条件：被减数在r0中，减数在r1中。;出口信息：ov=0时，差仍在r0中，ov=1时，溢出。;影响资源：psw、a、b、r2r7、位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;传回尾数的低字节movr0,adecr0mova,r3;传回尾数的高字节movr0,adecr0mova,r2;取结果的阶码movc,1fh;取结果的数符movacc.7,c;拼入阶码中movr0,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;阶码相同，对阶结束jbacc.7,eq3;阶符互异mova,r2;阶符相同，比较大小clrcsubba,r5jceq4eq2:clrc;第二操作数右规一次mova,r6;尾数缩小一半rrcamovr6,amova,r7rrcamovr7,aincr5;阶码加一orla,r6;尾数为零否？jnzeq;尾数不为零，继续对阶mova,r2;尾数为零，提前结束对阶movr5,asjmpas4eq3:mova,r2;判断第一操作数阶符jnbacc.7,eq2;如为正，右规第二操作数eq4:clrclcallrr1;第一操作数右规一次orla,r3;尾数为零否？jnzeq;不为零，继续对阶mova,r5;尾数为零，提前结束对阶movr2,aas4:jbf0,as5;尾数加减判断mova,r4;尾数相加adda,r7movr4,amova,r3addca,r6movr3,ajncas2ljmprr1;有进位，右规一次as5:clrc;比较绝对值大小mova,r4subba,r7movb,amova,r3subba,r6jcas6movr4,b;第一尾数减第二尾数movr3,aljmprln;结果规格化as6:cpl1fh;结果的符号与第一操作数相反clrc;结果的绝对值为第二尾数减第一尾数mova,r7subba,r4movr4,amova,r6subba,r3movr3,arln:mova,r3;浮点数规格化orla,r4;尾数为零否？jnzrln1movr2,#0c1h;阶码取最小值*？应为#c0hretrln1:mova,r3jbacc.7,rln2;尾数最高位为一否？clrc;不为一，左规一次lcallrl1sjmprln;继续判断rln2:clrov;规格化结束retrl1:mova,r4;第一操作数左规一次rlca;尾数扩大一倍movr4,amova,r3rlcamovr3,adecr2;阶码减一cjner2,#0c0h,rl1e;阶码下溢否？*应改为cjner2，#0bfh，rl1e；clramovr3,a;阶码下溢，操作数以零计movr4,amovr2,#0c1h;*应改为movr2，#0c0hrl1e:clrovretrr1:mova,r3;第一操作数右规一次rrca;尾数缩小一半movr3,amova,r4rrcamovr4,aincr2;阶码加一clrov;清溢出标志cjner2,#40h,rr1e;阶码上溢否？movr2,#3fh;阶码溢出setbovrr1e:ret;（）标号：功能：浮点数乘法;入口条件：被乘数在r0中，乘数在r1中。;出口信息：ov=0时，积仍在r0中，ov=1时，溢出。;影响资源：psw、a、b、r2r7、位1eh、1fh堆栈需求：字节fmul:lcallmvr0;将r0传送到第一工作区中mova,r0xrla,r1;比较两个操作数的符号rlcamov1fh,c;保存积的符号lcallmul0;计算积的绝对值ljmpmov0;将结果传回到r0中mul0:lcallmvr1;将r1传送到第二工作区中mul1:mova,r3;第一尾数为零否？orla,r4jzmul6mova,r6;第二尾数为零否？orla,r7jzmul5mova,r7;计算r3r4r6r7r3r4movb,r4mulabmova,bxcha,r7movb,r3mulabadda,r7movr7,aclraaddca,bxcha,r4movb,r6mulabadda,r7movr7,amova,baddca,r4movr4,aclrarlcaxcha,r3movb,r6mulabadda,r4movr4,amova,baddca,r3movr3,ajbacc.7,mul2;积为规格化数否？r7四舍五入mova,r7;左规一次rlcamovr7,alcallrl1mul2:mova,r7jnbacc.7,mul3incr4mova,r4jnzmul3incr3mova,r3jnzmul3movr3,#80hincr2mul3:mova,r2;求积的阶码adda,r5md:movr2,a;阶码溢出判断jbacc.7,mul4jnbacc.6,mul6movr2,#3fh;阶码上溢，设立标志setbovretmul4:jbacc.6,mul6mul5:clra;结果清零（因子为零或阶码下溢）movr3,amovr4,amovr2,#41hmul6:clrovret;（）标号：功能：浮点数除法;入口条件：被除数在r0中，除数在r1中。;出口信息：ov=0时，商仍在r0中，ov=1时，溢出。;影响资源：psw、a、b、r2r7、位1eh、1fh堆栈需求：字节fdiv:incr0mova,r0incr0orla,r0decr0decr0jnzdiv1movr0,#41h;被除数为零，不必运算clrovretdiv1:incr1mova,r1incr1orla,r1decr1decr1jnzdiv2setbov;除数为零，溢出retdiv2:lcallmvr0;将r0传送到第一工作区中mova,r0xrla,r1;比较两个操作数的符号rlcamov1fh,c;保存结果的符号lcallmvr1;将r1传送到第二工作区中lcalldiv3;调用工作区浮点除法ljmpmov0;回传结果div3:clrc;比较尾数的大小mova,r4subba,r7mova,r3subba,r6jcdiv4lcallrr1;被除数右规一次sjmpdiv3div4:clra;借用r0r1r2作工作寄存器xcha,r0;清零并保护之pushaccclraxcha,r1pushaccmova,r2pushaccmovb,#10h;除法运算，r3r4r6r7r0r1div5:clrcmova,r1rlcamovr1,amova,r0rlcamovr0,amova,r4rlcamovr4,axcha,r3rlcaxcha,r3movf0,cclrcsubba,r7movr2,amova,r3subba,r6anlc,/f0jcdiv6movr3,amova,r2movr4,aincr1div6:djnzb,div5mova,r6;四舍五入clrcrrcasubba,r3clraaddca,r1;将结果存回r3r4movr4,aclraaddca,r0movr3,apopacc;恢复r0r1r2movr2,apopaccmovr1,apopaccmovr0,amova,r2;计算商的阶码clrcsubba,r5lcallmd;阶码检验ljmprln;规格化;（）标号：功能：浮点数清零;入口条件：操作数在r0中。;出口信息：操作数被清零。;影响资源：a堆栈需求：字节fclr:incr0incr0clramovr0,adecr0movr0,adecr0movr0,#41hret;（）标号：功能：浮点数判零;入口条件：操作数在r0中。;出口信息：若累加器a为零，则操作数r0为零，否则不为零。;影响资源：a堆栈需求：字节fzer:incr0incr0mova,r0decr0orla,r0decr0jnzzeromovr0,#41hzero:ret;（）标号：功能：浮点数传送;入口条件：源操作数在r1中，目标地址为r0。;出口信息：r0=r1，r1不变。;影响资源：a堆栈需求：字节fmov:incr0incr0incr1incr1mova,r1movr0,adecr0decr1mova,r1movr0,adecr0decr1mova,r1movr0,aret;（.1）标号：r0功能：浮点数传送;入口条件：源操作数在r0中，目标地址为r1。;出口信息：r1=r0，r0不变。;影响资源：a堆栈需求：字节fmovr0:incr1incr1incr0incr0mova,r0movr1,adecr1decr0mova,r0movr1,adecr1decr0mova,r0movr1,aret;（）标号：功能：双字节十六进制定点数转换成格式化浮点数;入口条件：双字节定点数的绝对值在r0中，数符在位1fh中，整数部分的位数在a中。;出口信息：转换成格式化浮点数在r0中（三字节）。;影响资源：psw、a、r2、r3、r4、位1fh堆栈需求：字节dtof:movr2,a;按整数的位数初始化阶码mova,r0;将定点数作尾数movr3,aincr0mova,r0movr4,adecr0lcallrln;进行规格化ljmpmov0;传送结果到r0中;（）标号：功能：格式化浮点数转换成双字节定点数;入口条件：格式化浮点操作数在r0中。;出口信息：ov=1时溢出，ov=0时转换成功：定点数的绝对值在r0中（双字节），数符;在位1fh中，f0=1时为整数，cy=1时为一字节整数一字节小数，否则为纯小数。;影响资源：psw、a、b、r2、r3、r4、位1fh堆栈需求：字节ftod:lcallmvr0;将r0传送到第一工作区mova,r2jzftd4;阶码为零，纯小数jbacc.7,ftd4;阶码为负，纯小数setbcsubba,#10hjcftd1setbov;阶码大于，溢出retftd1:setbcmova,r2subba,#8;阶码大于否？jcftd3ftd2:movb,#10h;阶码大于，按双字节整数转换lcallftd8setbf0;设立双字节整数标志clrcclrovretftd3:movb,#8;按一字节整数一字节小数转换lcallftd8setbc;设立一字节整数一字节小数标志clrf0clrovretftd4:movb,#0;按纯小数转换lcallftd8clrov;设立纯小数标志clrf0clrcretftd8:mova,r2;按规定的整数位数进行右规*阶码是扩展后的值cjnea,b,ftd9mova,r3;将双字节结果传送到r0中movr0,aincr0mova,r4movr0,adecr0retftd9:clrclcallrr1;右规一次sjmpftd8;（）标号：功能：浮点码转换成格式化浮点数;入口条件：浮点码操作数在r0中。;出口信息：转换成的格式化浮点数仍在r0中。;影响资源：psw、a、b、r2r7、位1dh1fh堆栈需求：字节btof:incr0;判断是否为零。incr0mova,r0movr7,adecr0mova,r0movr6,adecr0orla,r7jnzbtf0movr0,#41h;为零，转换结束。retbtf0:mova,r0movc,acc.7mov1dh,c;保存数符。clr1fh;以绝对值进行转换。movc,acc.6;扩充阶码为八位。movacc.7,cmovr0,ajncbtf1adda,#19;是否小于？jcbtf2movr0,#41h;小于时以计。incr0movr0,#0incr0movr0,#0decr0decr0retbtf1:subba,#19jcbtf2mova,#3fh;大于时封顶。movc,1dhmovacc.7,cmovr0,aincr0movr0,#0ffhincr0movr0,#0ffhdecr0decr0retbtf2:clra;准备将码尾数转换成十六进制浮点数。movr4,amovr3,amovr2,#10h;至少两个字节。btf3:mova,r7adda,r7daamovr7,amova,r6addca,r6daamovr6,amova,r4rlcamovr4,amova,r3rlcamovr3,adecr2jnbacc.7,btf3;直到尾数规格化。mova,r6;四舍五入。adda,#0b0h;*加#80h，也可以clraaddca,r4movr4,aclraaddca,r3movr3,ajncbtf4movr3,#80h;*有进位右规一次incr2btf4:movdptr,#btfl;准备查表得到十进制阶码对应的浮点数。mova,r0adda,#19;计算表格偏移量。movb,#3mulabadda,dplmovdpl,ajncbtf5incdphbtf5:clra;查表。movca,a+dptrmovc,acc.6movacc.7,cmovr5,amova,#1movca,a+dptrmovr6,amova,#2movca,a+dptrmovr7,alcallmul1;将阶码对应的浮点数和尾数对应的浮点数相乘。movc,1dh;取出数符。mov1fh,cljmpmov0;传送转换结果。;（）标号：功能：格式化浮点数转换成浮点码;入口条件：格式化浮点操作数在r0中。;出口信息：转换成的浮点码仍在r0中。;影响资源：psw、a、b、r2r7、位1dh1fh堆栈需求：字节ftob:incr0mova,r0incr0orla,r0decr0decr0jnzftb0movr0,#41hretftb0:mova,r0movc,acc.7mov1dh,cclracc.7movr0,alcallmvr0movdptr,#bfl0;绝对值大于或等于时的查表起点。movb,#0;十的次幂。mova,r2jnbacc.7,ftb1movdptr,#btfl;绝对值小于时的查表起点。movb,#0edh;十的次幂。adda,#16jncftb1movdptr,#bfln;绝对值大于或等于时的查表起点。movb,#0fah;十的次幂。ftb1:clra;查表，找到一个比待转换浮点数大的整数幂。movca,a+dptrmovc,acc.6movacc.7,cmovr5,amova,#1movca,a+dptrmovr6,amova,#2movca,a+dptrmovr7,amova,r5;和待转换浮点数比较。clrcsubba,r2jbacc.7,ftb2;差为负数。jnzftb3mova,r6clrcsubba,r3jcftb2jnzftb3mova,r7clrcsubba,r4jcftb2jnzftb3movr5,b;正好是表格中的数。incr5;幂加一。movr6,#10h;尾数为。movr7,#0sjmpftb6;传送转换结果。ftb2:incdptr;准备表格下一项。incdptrincdptrincb;幂加一。sjmpftb1;ftb3:pushb;保存幂值。lcalldiv3;相除，得到一个二进制浮点数的纯小数。ftb4:mova,r2;取阶码。jzftb5;为零吗？clrc;lcallrr1;右规。sjmpftb4ftb5:popacc;取出幂值。movr5,a;作为十进制浮点数的阶码。lcallhb2;转换尾数的十分位和百分位。movr6,alcallhb2;转换尾数的千分位和万分位。movr7,amova,r3;四舍五入。rlcaclraaddca,r7daamovr7,aclraaddca,r6daamovr6,ajncftb6movr6,#10hincr5ftb6:incr0;存放转换结果。incr0mova,r7movr0,adecr0mova,r6movr0,adecr0mova,r5movc,1dh;取出数符。movacc.7,cmovr0,arethb2:mova,r4;尾数扩大倍。movb,#100mulabmovr4,amova,bxcha,r3movb,#100mulabadda,r3movr3,ajnchb21incbhb21:mova,b;将整数部分转换成码。movb,#10divabswapaorla,bretbtfl:db41h,0ech,1eh;1.0000e-19db45h,93h,93h;1.0000e-18db48h,0b8h,78h;1.0000e-17db4bh,0e6h,96h;1.0000e-16db4fh,90h,1dh;1.0000e-15db52h,0b4h,25h;1.0000e-14db55h,0e1h,2eh;1.0000e-13db59h,8ch,0bdh;1.0000e-12db5ch,0afh,0ech;1.0000e-11db5fh,0dbh,0e7h;1.0000e-10db63h,89h,70h;1.0000e-9db66h,0abh,0cch;1.0000e-8db69h,0d6h,0c0h;1.0000