pid汇编控制程序

;chen.asm
.include "vector.h" ; 建立中断向量表
.include "lf2407.h" ; 引用头部文件
.def _c_int0
.mmregs
.sect ".costab"
costab .include "cos.tab"
.sect ".firtab"
firtab .include "fir.tab"
****************************
;主程序
.text
_c_int0
CALL SYSINIT
CALL IOINIT
CALL EVAINIT
SYSINIT:
SETC INTM
SETC OVM
SETC SXM
CLRC XF ;用于检验是否进中断
SETC CNF ;B0区被配置为程序空间
SPM 0
LDP #0
SPLK #0007H,IMR ;使能中断优先级INT1、INT2、INT3
SPLK #0FFFFh,IFR ;清中断标志
LDP #07H
SPLK #0,TT
OUT TT,WSGR ;等待寄存器,读外部存储器时插入的时钟周期数目
********************************************
;变量初始化
SPLK #0h, count
SPLK #0h, index
SPLK #0, tmpAD
SPLK #0,VE0
SPLK #0,VE1
SPLK #0,VE2
SPLK #0,U
SPLK #0,PIDTMP2
;SPLK #0,U2
********************************************
RET

IOINIT:
LDP #DP_PF1 ;指向7000h～7080h区
SPLK #8005H,XINT1CR
LDP #DP_PF2
SPLK #21C4H,MCRA
;SPLK #0400H,MCRC
;SPLK #0FF00H,PADATDIR

;LACL MCRB
;OR #0FF00H
;SACL MCRB
RET
EVAINIT:

LDP #DP_EVA ;指向7400h～7480h区
SPLK #0200H,EVAIMRA ;使能T1UFINT_2 中断
SPLK #0FFFFh,EVAIFRA ;清EVA中断标志
SPLK #60C6H,GPTCONA
SPLK #3000,T1PR ;使定时器每0.1ms产生一次中断
SPLK #0,T1CNT
SPLK #02700H, COMCONA
SPLK #0A700H, COMCONA

SPLK #016H,ACTRA
SPLK #0474H,DBTCONA
SPLK #0,CMPR1
SPLK #0,CMPR2
SPLK #0842H, T1CON ;连续增减计数

SPLK #01H,EVAIMRB ;使能T2PINT中断
SPLK #0FFFFh,EVAIFRB
SPLK #9376,T2PR
SPLK #0,T2CNT
SPLK #4688,T2CMPR
SPLK #0D4AH,T2CON

ADINIT:
LDP #225 ;指向7080H～7100H
SPLK #4002H,ADCTRL1
SPLK #2002H,ADCTRL1
SPLK #0H,MAXCONV ;采1路信号
SPLK #0001H,CHSELSEQ1
SPLK #4100H,ADCTRL2
SPLK #0100H,ADCTRL2
******************************************
CLRC INTM ;开总中断
WAIT NOP
B WAIT
*****中断程序******************************
GISR1:

LDP #0E0H
LACC PIVR,1 ;读取外设中断向量寄存器（PIVR），并左移一位
ADD #PVECTORS ;加上外设中断入口地址XINT1_1同步中断入口,保护现场
BACC
GISR2: ;T1UFINT_2周期中断入口

LDP

#0E0H
LACC PIVR,1 ;读取外设中断向量寄存器（PIVR），并左移一位
ADD #PVECTORS ;加上外设中断入口地址
BACC
GISR3: ;T2PINT_3周期中断入口

LDP #0E0H
LACC PIVR,1 ;读取外设中断向量寄存器（PIVR），并左移一位
ADD #PVECTORS ;加上外设中断入口地址
BACC
XINT1_1:
LDP #07H
SPLK #0,count
SPLK #0,index
LDP #DP_PF1
SPLK #8005H,XINT1CR

LDP #DP_EVA
SPLK #0,T2CNT
SPLK #3888,T2CMPR
;LDP #0
;SPLK #0007h,IMR
;splk #0ffffh,ifr
CLRC INTM
RET

T1UFINT_2:
;AD开始
CHECK LDP #225 ;采样电源电流
BIT ADCTRL2,3 ;检测SEQ1是否处于空闲状态
BCND CHECK,TC ;等待空闲状态

LDP #0E1H
LACC RESULT0,10 ;左移10位装入累加器
LDP #07H
SACH AD ;高位存入AD
LACL AD
AND #03FFH ;取低10位，即采样值

SUB #512 ;减去偏移量，变为交流量
SACL AD

SPM 0
LT AD
LDP #18
MPY KCURRENT ;Q12
PAC
LDP #07
SACL AD ;定标为12

;输入filter

LT tmpAD ;q12
LDP #18
MPY QQ1 ;q15
PAC
LDP #07H ;yout(n)=(1-Q)*yout(n-1)+Q*x(n)
LT AD ;first older filter Q value is 0.9
LDP #18
MPY QQ2 ;q15
APAC
LDP #07H
SACH tmpAD,1 ;q12
********************************************
;查表做乘
COSTABLE
LDP #07H
;splk #0h,count ;取0～200
;splk #0h,index ;取0～50
LACC count
SUB #25
BCND second_0,EQ
BCND second_1,GT
first_1
COS ;这是什么东西????????????
LDP #07H
LACC index
ADD #01
SACL index
LACC count
ADD #01
SACL count
B END0
second_0
SPLK #25,index
second_1
LACC count
SUB #50
BCND third_0,EQ
BCND third_1,GT
COS
LDP #07H
LACC costeta
NEG
SACL costeta
LACC index
SUB #01
SACL index
LACC count
ADD #01

SACL count
B END0
third_0
SPLK #0,index
third_1
LACC count
SUB #75
BCND fourth_0,EQ
BCND fourth_1,GT
COS
LDP #07H
LACC costeta
NEG
SACL costeta
LACC index
ADD #01
SACL index
LACC count
ADD #01
SACL count
B END0
fourth_0
SPLK #25,index
fourth_1
LACC count
SUB #100
BCND five,EQ
BCND five,GT
COS
LDP #07H
LACC index
SUB #01
SACL index
LACC count
ADD #01
SACL count
B END0
five
SPLK #0h,index
SPLK #32767,costeta
END0
LDP #07H
LT costeta
ZAC ;这是什么东西????????????
MPY tmpAD
PAC
SACH PRODUCT ;Q11
**************************************
;FIR滤波器
LDP #07H
LACL PRODUCT
LDP #20 ;指向0A00H～0A80H
SACL 0
LARP AR1 装载ARP=1??????
LRLK AR1,0AC8h ;指向块的底部??????
MPY #0
ZAC
RPT #200
MACD 0FE00h,*-
APAC ;PREG加至累加器
LDP #07H
SACH FIRO ;变为Q10定标
;splk #0,FIRO
****************************************
;PID控制

PID SPM 0
SETC SXM ;允许符号扩展
LDP #18
LACL VREF
LDP #07H ;Vref，Q10
SUB FIRO ;V，Q10
SACL VE2 ;e(k)= Vref－V，Q10
SUB VE1 ;e(k-1)，Q10
SACL PIDTMP1 ;Delta(e(k))=e(k)-e(k－1)，Q10
SACL TEMPE1 ;保存模糊控制输入误差量 q10
SUB VE1
ADD VE0 ;e(k－2)
SACL PIDTMP3 ;Delta(e(k)) －Delta(e(k－1))= e(k)-2e(k－1)+e(k－2),Q10
SACL TEMPE2 ;保存模糊控制输入误差变化量q12
LDP #18
LT KP ;Kp, 0.2q(15)
LDP #07H
MPY PIDTMP1
PAC ;Kp*Delta(e(k))，
SACH PIDTMP1,1 ;Kp*Delta(e(k))q(10)
;LDP #18
;LT KI ;Ki，100q(8)
;LDP #07H
;MPY VE2
;PAC ;Ki*e(k)，q(18)
;RPT #7
;SFR ;Ki*e(k)
;SACL PIDTMP2 ;Ki*e(k),q(10)

LACL U ;u(k－1)?
ADD PIDTMP1 ;Delta(u(k))
;ADD PIDTMP2
SACL U ;u(k)，Q10

LACL VE1 ;平移保存
SACL VE0 ;e(k－2)=e(k－1)
LACL VE2
SACL VE1
***************************************

*****
LACC U ;Q10
ADD #3C00H ;向上平移15V(Q10)，变为0～30
SACL PIDO ;Q10

SUB #07800H ;30V，
BCND P1,LEQ
SPLK #07800H,PIDO ;上限幅
P1 BIT PIDO,0
BCND P2,NTC
SPLK #0,PIDO ;下限幅
P2 LACL PIDO ;转化为时钟周期
RPT #9
SFR
SACL PIDO ;转化为Q0
LT PIDO
MPY #64H ;100
PAC
SACL PIDO
;LDP #07H
;SPLK #2000,PID10
;BLDD PID10,#CMPR1
;LACC PID10
;ADD #20 ;脉宽两沿各减小8us
;SACL PID11
;BLDD PID11,#CMPR2
LACC PIDO
ADD #10 ;脉宽两沿各减小8us
SACL PID1
BLDD PIDO,#CMPR1
BLDD PID1,#CMPR2
CLRC XF
********************************************
LDP #225
SPLK #4100H,ADCTRL2
SPLK #0100H,ADCTRL2 ;恢复AD现场
********************************************



LDP #DP_EVA ;指向7400h～7480h区
SPLK #0,T1CNT
SPLK #0200H,EVAIMRA ;使能T1UFINT中断
SPLK #0FFFFh,EVAIFRA
CLRC INTM
RET

T2PINT:

LDP #DP_EVA
SPLK #5488,T2CMPR
SPLK #01H,EVAIMRB ;使能T2PINT 中断
SPLK #0FFFFh,EVAIFRB
CLRC INTM
RET
;假中断程序
PHANTOM
KICK_DOG ;复位看门狗
RET
.END