digital_pid.asm

《DSP嵌入式系统开发典型案例》此书的源代码

   
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;   实用数字PID控制程序 
;;;;   本程序的文件名是Digital_PID.asm
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
		.title  "Digital_PID.asm" 
		
		.include "lf2407_regs.h"  ;引用头部文件
		.include "Digital_PID_vec.asm" 
		 
		.def _cy_begin
		
Kp_init           .set  100h 
Ki_high_init      .set  00h 
Ki_low_init       .set  1000h
Kd_init           .set  05h
PID_ref_init      .set  500
PID_output_MAX    .set  07FFFh
PID_output_MIN    .set  0h
         
        
        .data 
ADRESULT      .word  00h 
PID_input      .word  00h   
PID_output     .word  00h
PID_output1     .word  00h 
PID_reference  .word  00h
PID_e0     .word  00h 
PID_e1     .word  00h
PID_e2     .word  00h
 
Kp         .word  00h
Ki_high    .word  00h
Ki_low      .word  00h
Kd            .word  00h
A_coeff_high   .word  00h
A_coeff_low    .word  00h
B_coeff        .word  00h

tmp1_high   .word  00h 
tmp1_low   .word  00h
tmp2_high   .word  00h
tmp2_low   .word  00h 
tmp3   .word  00h 
e0_sign   .word  00h
abs_e0    .word  00h

		
		.text
_cy_begin:
		NOP
		CALL  system_init   ;系统初始化程序 
		CALL  PID_init
    	CALL  cy_AD_init   ;EVB模块PWM初始化程序
cy_LOOP:
		NOP
		NOP
		B	  cy_LOOP

system_init: ;系统初始化程序 
		SETC INTM
        CLRC OVM
        CLRC SXM
        CLRC CNF   ;B0区被配置为数据空间
        LDP  #DP_PF1    ;指向7000h～7080h区
        SPLK #081FEh,SCSR1 ;时钟4倍频，CLKIN=6 M，CLKOUT=24 M
        SPLK #0E8h,WDCR ;;用#068h也是可以的 ;不使能WDT
        LDP  #0
        SPLK #0001h,IMR  ;使能中断第1级INT1
        SPLK #0FFFFh,IFR   ;清全部中断标志
        RET
        
cy_AD_init: ;A/D转换模块初始化程序
		LDP	  #DP_EVB				;指向7080h～7100h区
		SPLK  #0000h, T4CNT
		SPLK  #0176h, T4PR
		SPLK  #0400h, GPTCONB 
		SPLK  #0170Ch, T4CON
		SPLK  #0FFFFh, EVBIFRA
		SPLK  #0FFFFh, EVBIFRB
		SPLK  #0FFFFh, EVBIFRC 
		SPLK  #00000h, EVBIMRA
		SPLK  #00000h, EVBIMRB
		SPLK  #00000h, EVBIMRC 
		
		LDP   #DP_PF2
		SPLK  #0000h, ADCCTRL1   
		SPLK  #8404h, ADCCTRL2
		SPLK  #0001h, MAXCONV
		SPLK  #0000h, CHSELSEQ1 
		SPLK  #0000h, CHSELSEQ2
		SPLK  #0000h, CHSELSEQ3
		SPLK  #0000h, CHSELSEQ4
		
		CALL   AD_Start				
		
		LDP   #4
		SPLK  #0000h,  ADRESULT
		RET
		
AD_Start:
        LDP   #DP_EVB
        LACL  T4CON
        OR    #0040h
        SACL  T4CON
        RET

PID_Control: 
        SETC  SXM
        SETC  OVM
        SPM   #0
        LDP   #4
        LACL  ADRESULT
        SACL  PID_input
        LACC  Ki_high, 16
        ADDS  Ki_low
        ADD   Kp, 16
        ADD   Kd, 16
        SACH  A_coeff_high
        SACL  A_coeff_low
        LACC  Kd, 16
        SFL
        ADD   Kp, 16
        SACH  B_coeff
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
		LACC  PID_reference
		SUB   PID_input
		SACL  PID_e0
		LT    Kd
		MPY   PID_e2
		PAC
		LT    B_coeff
		MPY   PID_e1
		SPAC
		SACH  tmp1_high, 1
		SACL  tmp1_low,  1
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
        LACC  PID_e0
        SACL  e0_sign
        ABS
        SACL  abs_e0
        LT    abs_e0
        MPYU  A_coeff_low
        SPH   tmp2_low
        MPYU  A_coeff_high
        PAC
        ADDS  tmp2_low
        SACH  tmp2_high, 1
        SACL  tmp2_low,  1
        LACC  e0_sign
        BCND  cy_DONE,  GT
        LACC  tmp2_high, 16
        ADDS  tmp2_low
        NEG
        SACH  tmp2_high
        SACL  tmp2_low
cy_DONE:
        LACC  tmp1_high, 16
        ADDS  tmp1_low
        ADDS  tmp2_low
        ADD   tmp2_high, 16
        SACH  tmp3
        LACC  PID_output1, 16
        ADD   tmp3, 16
        SACH  PID_output
        ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
        LACC  PID_output
        SUB   #PID_output_MAX
        BCND  greater_MAX, GT
        LACC  PID_output
        SUB   #PID_output_MIN
        BCND  less_MIN, LT
        B     PID_end
greater_MAX:
        SPLK  #PID_output_MAX, PID_output
        B     PID_end
less_MIN:
        SPLK  #PID_output_MIN, PID_output
PID_end:
        LDP   #4
        LACC  PID_e1
        SACL  PID_e2 
        LACC  PID_e0
        SACL  PID_e1
        LACC  PID_output
        SACL  PID_output1
        CLRC  SXM
        RET
        
PID_init:
        LDP   #4
        SPLK  #Kp_init, Kp 
        SPLK  #Ki_high_init, Ki_high 
        SPLK  #Ki_low_init, Ki_low
        SPLK  #Kd_init, Kd
        SPLK  #PID_ref_init, PID_reference
        SPLK  #0, PID_e2 
        SPLK  #0, PID_e1
        SPLK  #0, PID_e0
        SPLK  #0, PID_output1
        SPLK  #0, PID_output
        RET
        
        
GISR1:					;优先级INT1中断入口
		;保护现场
		LDP		#DP_PF1
		LACC	PIVR,1				;读取外设中断向量寄存器（PIVR），并左移一位
		ADD		#PVECTORS			;加上外设中断入口地址
		BACC						;跳到相应的中断服务子程序

ADCINT_ISR:
        CLRC   SXM
        LDP    #4
        LAR    AR2, #RESULT0
        MAR    *, AR2
        LACC   *, 10
        SACH   ADRESULT
        CALL   PID_Control
        CLRC   INTM  ;开总中断，因为一进中断就自动关闭总中断
        RET      ;中断返回
		

PHANTOM:   ;假中断程序	
        LDP #DP_PF1
        SPLK #05555h, WDKEY	;复位看门狗
		SPLK #0AAAAh, WDKEY
		RET
		.end