;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;   實用數字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