數(shù)字信號處理技術(shù)_03PID算法(PID算法的DSP實現(xiàn))

1、第5章 PID算法的DSP實現(xiàn)5.1 PID控制系統(tǒng)5.1.1 PID控制系統(tǒng)硬件電路的組成圖5.1 PID控制系統(tǒng)的硬件電路5.1.2 PID控制過程圖5.2 基于LF2407的PID控制器的實現(xiàn)框圖5.2 AD80415.2.1 AD8041簡介5.2.2 AD8041的引腳與引腳功能5.3 AD72375.3.1 AD7237簡介5.3.2 AD7237的工作原理5.4 PID控制5.4.1 PID控制方法簡介5.4.2 PID控制方法的實現(xiàn)下面介紹一個典型的數(shù)字PID控制器。圖5.5是數(shù)字PID控制器的原理框圖。圖5.5 PID控制器的原理框圖PID控制的離散形式，如式（5.2）所示。

2、 （5.2）增量式PID控制算式如式（5.4）所示。 (5.4)令，則 (5.5)5.4 定點運算基礎(chǔ)5.4.1定標的概念1. 數(shù)的定標2. 溢出及處理方法3. 舍入與截尾5.4.2定點運算1. 加法/減法定點運算的C語言模擬2. 乘法定點運算的C語言模擬3. 除法定點運算的C語言模擬4. 程序變量的Q值確定5.4.3 TMS320LF2407的定點運算1. 定點乘法2. 定點加法5.5 PID算法程序?qū)崿F(xiàn)5.5.1 PID控制算法匯編主程序公式中變量和程序中變量的對應(yīng)關(guān)系及意義表公式中的變量意義描述程序中的對應(yīng)變量公式中的變量意義描述程序中的對應(yīng)變量r(k)PID控制器的參考輸入量PID_i

3、nputKi積分增益系數(shù)高位字Ki_highc(k)PID控制器的實際輸入量PID_referenceKi積分增益系數(shù)低位字Ki_lowu(k)PID控制器的當前控制量PID_outputKd微分增益系數(shù)Kdu(k-1)PID控制器的上次控制量PID_output1AA系數(shù)高位=Kp+Ki+KdA_coeff_highe(k)當前偏差量PID_e0AA系數(shù)低位=Kp+Ki+KdA_coeff_lowe(k-1)上次偏差量PID_e1BB系數(shù)=Kp+2*KdB_coeffe(k-2)上上次偏差量PID_e2CC系數(shù)=Kd用Kd代替Kp比例增益系數(shù)Kp1. 所需的復(fù)位和中斷向量定義文件“vecto

4、rs. asm”/ 該文件利用匯編語言代碼定義了復(fù)位和中斷向量 .title“vectors .asm”；此文件名為vectors .asm.sect“.vectors”；定義主向量段reset:b_c_int0；復(fù)位向量int1:bGISR1；高優(yōu)先級ADC中斷屬于INT1int2:bPHANTOMint3:bPHANTOMint4:bPHANTOMint5:bPHANTOMint6:bPHANTOMreservedbPHANTOMsw_int8bPHANTOMsw_int31bPHANTOM.sect“. pvectors”；定義子向量段PVECTORSbPHANTOM；保留向量地址偏移量

5、0000HbPHANTOM；保留向量地址偏移量0001HbPHANTOM；保留向量地址偏移量0002HbPHANTOM；保留向量地址偏移量0003HbADCINT_ISR；向量地址偏移量0004H為ADC中斷bPHANTOM；保留向量地址偏移量0005HbPHANTOM；保留向量地址偏移量00041H2. 主程序Digital_PID.asm；實用數(shù)字PID控制程序.title Digital_PID.asm；匯編偽指令，定義程序的文件名.include register.h；引用頭部文件.include vectors.asm；引用中斷向量表.def _cy_begin；定義程序的入口地址K

6、p_init .set 100H；比例增益系數(shù)Kp的初始值Ki_high _init .set 00H；積分增益系數(shù)Ki的高位初始值Kp_low_init .set 1000H；積分增益系數(shù)Ki的低位初始值Kd_init .set 05H；微分增益系數(shù)Kd的初始值PID_ref_init .set 500；數(shù)字PID控制器的參考輸入初始值；本程序?qū)崿F(xiàn)電機速度PID控制，設(shè)置參考輸入是500轉(zhuǎn)/分鐘PID_output_MAX .set 07FFFH；數(shù)字PID控制器輸出控制量最大值限幅PID_output_MIN .set 0H；數(shù)字PID控制器輸出控制量最小值限幅.dataADRESULT

7、.word 00H；存儲當前A/D轉(zhuǎn)換的結(jié)果PID_input .word 00H；存儲變量ADRESULT，也即當前實際輸入量c(k)PID_output .word 00H；存儲數(shù)字PID控制器的當前控制量u(k)PID_output1 .word 00H；存儲數(shù)字PID控制器的上次控制量u(k-1)PID_reference .word 00H；存儲數(shù)字PID控制器的當前參考輸入量r(k)PID_e0 .word 00H；存儲數(shù)字PID控制器的當前偏差量e(k)，e(k)=r(k)-c(k)PID_e1 .word 00H；存儲數(shù)字PID控制器的上次偏差量e(k-1)PID_e2 .wo

8、rd 00H；存儲數(shù)字PID控制器的上上次偏差量e(k-2)Kp .word 00H；存儲比例增益系數(shù)KpKi_high .word 00H；存儲積分增益系數(shù)Ki的高位字Ki_low .word 00H；存儲積分增益系數(shù)Ki的低位字Kd .word 00H；存儲微分增益系數(shù)KdA_coeff_high .word 00H；存儲A系數(shù)高位A_coeff_low .word 00H；存儲A系數(shù)低位B_coeff .word 00H；存儲B系數(shù)tmp1_high .word 00H；暫存單元1的高位字tmp1_low .word 00H；暫存單元1的低位字tmp2_high .word 00H；暫存

9、單元2的高位字tmp2_low .word 00H；暫存單元2的低位字tmp3 .word 00H；暫存單元3e0_sign .word 00H；用來存儲當前偏差量e(k)的符號abs_e0 .word 00H；用來存儲當前偏差量e(k)的絕對值.text_cy_begin:NOPCALL system_init；調(diào)用系統(tǒng)初始化子程序CALL PID_init；調(diào)用數(shù)字PID控制的初始化子程序CALL cy_AD_init；調(diào)用DSP A/D轉(zhuǎn)換器的初始化子程序cy_LOOP:；主程序進入循環(huán)，在高優(yōu)先級的ADCINT控制下進行；ADCINT一次采樣一個實際值，并進行一次PID控制NOPNOP

10、B cy_LOOPsystem_init:；系統(tǒng)初始化子程序SETC INTM；關(guān)閉系統(tǒng)的全局中斷CLRC OVM；累加器正常溢出CLR SXM；抑制符號擴展CLRC CNF；DARAM被映射到數(shù)據(jù)存儲空間B0區(qū) LDP #DP_PF1 ；指向7000H707FH外設(shè)寄存器區(qū) SPLK #81FEH,SCSR1；CLKIN=6M Hz，CLKOUT=4*CLKIN=24M Hz SPLK #00E8H,WDCR；禁止看門狗 LDP #0 ；指向數(shù)據(jù)頁的第0區(qū) SPLK #0001H,IMR；使能INT1 SPLK #0FFFFH,IFR；清除中斷標志 RETcy_AD_init:LDP #DP

11、_EVB ；A/D轉(zhuǎn)換模塊初始化子程序SPLK #0000H,T4CNT；定時器4的計數(shù)器清0 SPLK #0176H,T4PR；定時器的周期寄存器設(shè)置為374，周期計數(shù)1ms SPLK #0400H,GPTCONB；位109=10，通用定時器的周期中斷啟動模數(shù)轉(zhuǎn)換；位6=0，禁止所有定時器的比較輸出SPLK #0160CH,T4CON；位1211=10，連續(xù)增計數(shù)模式；位108=110，輸入時鐘預(yù)定標系數(shù)為64；位=6，禁止定時器操作；位54=00，使用內(nèi)部CPU時鐘 SPLK #0FFFFH,EVBIFRA SPLK #0FFFFH,EVBIFRB SPLK #0FFFFH,EVBIFRC

12、 SPLK #0000H,EVBIMRA SPLK #0001H,EVBIMRB；僅使能EVB的定時器4的周期中斷T4PINT SPLK #0000H,EVBIMRCLDP #DP_PF2；指向7080H70FFH外設(shè)寄存器區(qū)SPLK #0000H,ADCCTRL1；配置ADC控制寄存器1；位118=0000，ADC的采樣時間=2*CPU時鐘周期；位7=0，時鐘預(yù)定標系數(shù)CPS=0，ADC輸入時鐘頻率=CPU時鐘頻率；位6=0，采用啟動/停止模式；位5=0，ADC的中斷請求為高優(yōu)先級；位4=0，ADC轉(zhuǎn)換器采用雙排序器工作模式；位0=0，禁止自測試使能模式SPLK #8404H,ADCCTRL

13、2；配置ADC控制寄存器2；位15=1，不使能ADC轉(zhuǎn)換器的級連模式；位1110=01，ADC的排序器1在中斷標志位1置1時立即申請中斷；位9=0，目前ADC的排序器1無中斷事件發(fā)生；位32=01，ADC的排序器2在中斷標志位2置1時立即申請中斷；位0=0，ADC的排序器2不能被EVB觸發(fā)源啟動 SPLK #0001,MAXCONV；只有一路A/D采樣，所以最大轉(zhuǎn)換通道設(shè)為1 SPLK #0000H,CHSELSEQ1；配置輸入通道選擇排序控制寄存器 SPLK #0000H,CHSELSEQ2；使用通道號0 SPLK #0000H,CHSELSEQ3 SPLK #0000H,CHSELSEQ4

14、 CALL AD_ Start；配置完所有寄存器后，啟動A/D轉(zhuǎn)換 LDP #4；指向200H27FH數(shù)據(jù)存儲器的B0區(qū) SPLK #0000H,ADRESULT；初始化用戶變量ADRESULT，存轉(zhuǎn)換結(jié)果 RETAD_Start:；啟動A/D轉(zhuǎn)換信號LDP #DP_EVBLACL T4CON；把定時器4的控制寄存器位6（TENABLE）置1OR #0040H；使能定時器4的操作SACL T4CONRETPID_Control:；根據(jù)輸入值進行PID算法的計算SETC SXM；進行符號擴展SETC OVM；置符號擴展模式為1，溢出時將使用最大正值或負值SPM #0；表示乘法器不移位直接裝入CA

15、LULDP #4；指向用戶變量區(qū)200H27FHLACL ADRESULT；把本次A/D轉(zhuǎn)換的結(jié)果存儲到變量PID_input中SACL PID_inputLACC Ki_high,16；把積分增益系數(shù)Ki的高位字左移16位加到ACCADDS Ki_low；把Ki的高位字和低位字拼接起來加載到ACCADD Kp,16；ACC=Kp+KiADD Kd,16；ACC=Kp+Ki+KdSACH A_coeff_high；系數(shù)A高位字=高位(Kp+Ki+Kd)SACL A_coeff_low；系數(shù)A低位字=低位(Kp+Ki+Kd)LACC Kd,16SFL；此時，ACC=2*KdADD Kp,16；A

16、CC=2*Kd+KpSACH B_coeff；系數(shù)B=2*Kd+KpLACC PID_reference；加載輸入?yún)⒖贾礢UB PID_input；ACC=PID_reference-PID_inputSACL PID_e0；本次輸入的偏差量，e(k)=r(k)-c(k)LT KdMPY PID_e2；Kd*e(k-2)PAC；把乘積寄存器移位后加載到累加器,此處移位值是0LT B_coeff；把B系數(shù)加載到TREG寄存器中MPY PID_e1；B*e(k-1)=(2*Kd+Kp)*e(k-1)SPAC；此時ACC=Kd*e(k-2)-(2*Kd+Kp)*e(k-1)SACH tmp1_hig

17、h,1；把ACC的高位字放到暫存字tmp1_high中SACH tmp1_low,1；把ACC的低位字放到暫存字tmp1_low中；此處開始求最后一項增量，也即當前增量LACC PID_e0；加載當前偏差量e(k)到累加器ACC中SACL e0_sign；把e(k)存儲到變量e0_sign,便于后面進行符號判斷ABS；對e(k)取絕對值，為無符號乘法做準備SACL abs_e0LT abs_e0；把|e(k)|加載到TREG寄存器中MPYU A_coeff_low；無符號乘法，A低位字*|e(k)|SPH tmp2_low；把乘積寄存器PREG的高16位存儲到變量tmp2_lowMPYU A_

18、coeff_high；無符號乘法，A高位字*|e(k)|PAC；把A高位字*|e(k)|的乘積加載到累加器ADDS tmp2_low；ACC=A高位字*|e(k)|+A低位字*|e(k)|SACH tmp2_high,1；把該結(jié)果存儲到暫存器變量tmp2_high和tmp2_lowSACH tmp2_low,1LACC e0_sigh；判斷當前偏差量e(k)的符號BCND cy_DONE,GT；當e(k)0時，不必再進行處理，跳轉(zhuǎn)LACC tmp2_high,16；當e(k)=0時ADDS tmp2_low；使ACC=系數(shù)A高位字*|e(k)|+系數(shù)A低位字*|e(k)|NEG；因為e(k) PM PAGE 0.vectors: VECSPAGE 0.pvectors: PVECSPAGE 0.data: BLOCK_B0PAGE 15.5.4 PID控制算法的命令文件分析(1) 偽指令“MEMORY”定義了程序存儲器（PAGE 0）、數(shù)據(jù)存儲器（PAGE 1）。程序存儲器包括三個區(qū)間：VECS、PVECS、PM。其中，VECS區(qū)間的起始地址為0000H，區(qū)間長