foc.asm

该软件是关于交流异步电动机磁场定向调速控制的汇编程序

b limiterq
pos_satq
lacc #Vmax ;set ACC to pos saturation
limiterq
sacl vSqref ;Save ACC as reference value
sub upi
sacl elpi ;elpi=vSqref-upi, 4.12 format
lt elpi
mpy Kcor ;change to dma
pac
lt epiq
mpy Ki ;change to dma
apac
add xiq,12
sach xiq,4 ;xiq=xiq+epiq*Ki+elpi*Kcor, 4.12 f
***************************************************************
* END q-axis regulator with integral component correction
***************************************************************
***********************************************************
* d-axis current regulator with integral component
* correction
* (iSd,iSdref)->(vSdref)
;以下这一小节的代码和2407是一样的，可直接套用在2407中
***********************************************************
lacc iSdref;d轴即直轴相当于2407中的M轴，所以iSdref=2407中的IMREF
sub iSd
sacl epid ;epid=iSdref-iSd, 4.12 format
lacc xid,12
lt epid
mpy Kpi
apac
sach upi,4 ;upi=xid+epid*Kpi, 4.12 format
bit upi,0
bcnd upimagzerod,NTC
lacc #Vmin
sub upi
bcnd neg_satd,GT ;if upi<Vmin branch to saturate
lacc upi ;upi value valid
b limiterd
neg_satd
lacc #Vmin ;set acc to neg saturation
b limiterd
upimagzerod ;value was positive
lacc #Vmax
sub upi
bcnd pos_satd,LT ;if upi>Vmax branch to saturate
lacc upi ;upi value valid
b limiterd
pos_satd
lacc #Vmax ;set acc to pos saturation
limiterd
sacl vSdref ;store ACC as reference value
sub upi
sacl elpi ;elpi=vSdref-upi, 4.12 format
lt elpi
mpy Kcor
pac
lt epid
mpy Ki
apac
add xid,12
sach xid,4 ;xid=xid+epid*Ki+elpi*Kcor, 4.12 f
***************************************************************
* END d-axis regulator with integral component correction
***************************************************************
***************************************************************
* Inverse Park transformation
* (d,q) -> (alfa,beta)
* vSbe_ref = vSqref * cos(Teta_cm)+ vSdref * sin(Teta_cm)
* vSal_ref =-vSqref * sin(Teta_cm) + vSdref * cos(Teta_cm)
;与2407代码完全一致，可直接套用
***************************************************************
lacc #0
lt vSdref
mpy sinTeta_cm
lta vSqref
mpy cosTeta_cm
mpya sinTeta_cm
sach vSbe_ref,4
;vSbe_ref=vSqref*cosTeta_cm+vSdref*sinTeta_cm
lacc #0
lt vSdref
mpys cosTeta_cm
apac
sach vSal_ref,4
;vSal_ref=vSdref*cosTeta_cm-vSqref*sinTeta_cm
***************************************
* END Inverse Park transformation
***************************************
init
*******************************************
* SPACE VECTOR Pulse Width Modulation
*******************************************
*** sector calculation***
****************************************************
* Vref1 = vSbe_ref
* Vref2 = (-vSbe_ref + sqrt(3) * vSal_ref) / 2
* Vref3 = (-vSbe_ref - sqrt(3) * vSal_ref) / 2
;以下指令计算Vref1\Vref2\Vref3三个值
****************************************************
lt vSal_ref
mpy #SQRT32
pac
sub vSbe_ref,11
sach Vref2,4 ;4.12 format(指4位整数部分，12位小数部分),计算Vref2,Vref2= (-vSbe_ref + sqrt(3) * vSal_ref) / 2
pac  ;此句执行后，累加器ACC中为SQRT32*vSal_ref                       
neg  ;取负
sub vSbe_ref,11;此时累加器ACC中为-SQRT32*vSal_ref-vSbe_ref=Vref3
sach Vref3,4 ;4.12 format
lacl vSbe_ref
sacl Vref1 ;4.12 format此句的意思是令vSbe_ref=Vref1
****************************************************
* END reference voltage for sector calculation
****************************************************
;以下求取X\Y\Z的值
lt VDCinvT;  VDCinvT=245h,其计算过程参考手册内容P37
mpy #SQRT32
pac
sach tmp,4 ;tmp=VDCinvT*SQRT32, 4.12 format
lt tmp
mpy vSbe_ref
pac             ;此时累加器ACC中为VDCinvT*SQRT32*vSbe_ref
sach X,4 ;tmp*vSbe_ref, 4.12 format
lacc X ;ACC = vSbe_ref*VDCinvT*SQRT32
sacl tmp1 ;tmp1=vSbe_ref*VDCinvT*SQRT32, 4.12 format
sacl X,1 ;左移一位表示乘以2，则计算出X=(2*SQRT32*vSbe_ref*VDCinvT), 4.12 format
lt VDCinvT
splk #1800h,tmp ;3/2, 4.12 format(1800h即0001100000000000为十进制1.5即3/2的Q12格式)
mpy tmp ;implement mpy #01800h
pac
sach tmp,4 ;tmp=(3/2)*VDCinvT, 4.12 format
lt tmp
mpy vSal_ref
pac
sach tmp,4 ;tmp=(3/2)*VDCinvT*vSal_ref, 4.12 format
lacc tmp ;reload ACC with
;(3/2)*VDCinvT*vSal_ref
add tmp1 ;tmp1=vSbe_ref*VDCinvT*SQRT32,
;4.12 format
sacl Y ;得到Y=SQRT32*VDCinvT*vSbe_ref+(3/2)*VDCinvT*vSal_ref,
;4.12 format
sub tmp,1;先将tmp内容左移1位(乘2),再被ACC减去
sacl Z ;得到Z=SQRT32*VDCinvT*vSbe_ref-(3/2)*VDCinvT*vSal_ref,
;4.12 format
*** 60 degrees sector determination
lacl #0
sacl sector
lacc Vref1
bcnd Vref1_neg,LEQ ;If Vref1<0 do not set bit 1 of sector
lacc sector
or #1
sacl sector
Vref1_neg
lacc Vref2
bcnd Vref2_neg,LEQ ;If Vref2<0 do not set bit 2 of sector
lacc sector
or #2
sacl sector
Vref2_neg
lacc Vref3
bcnd Vref3_neg,LEQ ;If Vref3<0 do not set bit 3 of sector
lacc sector
or #4
sacl sector;  根据240手册的说明，sector=A+2B+4C
Vref3_neg
*** END 60 degrees sector determination
*** T1 and T2 (= t1 and t2) calculation depending on the
*** sector number
lacl sector
sub #1
bcnd no1,NEQ;如果sector=1，则向下执行t1=Z,t2=Y
lacc Z
sacl t1
lacc Y
sacl t2
b t1t2out
no1 ;sector不为1
lacl sector
sub #2   
bcnd no2,NEQ;看sector是否为2
lacc Y
sacl t1
lacc X
neg       ;sector=2则t1=Y,t2=-X
sacl t2
b t1t2out
no2      ;sector不为2
lacl sector
sub #3
bcnd no3,NEQ;看sector是否为3
lacc Z
neg
sacl t1
lacc X
sacl t2    ;sector=3,则t1=-Z,t2=X
b t1t2out
no3 
lacl sector;sector不为3
sub #4
bcnd no4,NEQ;看sector是否为4
lacc X
neg
sacl t1
lacc Z
sacl t2  ;sector=4,则t1=-X,t2=Z
b t1t2out
no4       ;sector不为4
lacl sector
sub #5
bcnd no5,NEQ;看sector是否为5
lacc X
sacl t1
lacc Y
neg
sacl t2  ;sector=5,则t1=X,t2=-Y
b t1t2out
no5 ;sector不为5，即为6
lacc Y
neg
sacl t1
lacc Z
neg
sacl t2  ;sector=6则t1=-Y,t2=-Z
t1t2out
*** END t1 and t2 calculation
lacc t1 ;if t1+t2>PWMPRD we have to saturate t1 and t2
add t2
sacl tmp;tmp=t1+t2
sub #PWMPRD
bcnd nosaturation,LT,EQ;看t1+t2-PWMPRD是否大于零，小于零则跳转到不饱和段nosaturation
*** t1 and t2 saturation
lacc #PWMPRD,15 ;divide PWMPRD by (t1+t2)
rpt #15
subc tmp;subc这里用作除法指令，重复subc16次表示ACC/tmp,商放在累加器低16位
sacl tmp
lt tmp ;calculate saturate values of t1 and t2
mpy t1 ;t1 (saturated)=t1*(PWMPRD/(t1+t2))
pac
sach t1,1
mpy t2 ;t2 (saturated)=t2*(PWMPRD/(t1+t2))
pac
sach t2,1
*** END t1 and t2 saturation
nosaturation
*** taon,tbon and tcon calculation
lacc #PWMPRD ;calculate the commutation
;instants taon, tbon and tcon
sub t1 ;of the 3 PWM channels
sub t2 
sfr;右移一位表示除以2
sacl taon;taon=(PWMPRD-t1-t2)/2
add t1 ;tbon=taon+t1
sacl tbon
add t2 ;tcon=tbon+t2
sacl tcon
*** END taon,tbon and tcon calculation
*** sector switching，比较寄存器的内容即占空比是多少，根据240手册的P38表26确定。
lacl sector ;depending on the sector number we have
sub #1 ;to switch the calculated taon, tbon and tcon
bcnd nosect1,NEQ ;to the correct PWM channel
bldd tbon,#CMPR1 ;若sector=1,将tbon的内容传到CMPR1中
bldd taon,#CMPR2 ;  
bldd tcon,#CMPR3 ;
b dacout
nosect1;sector不为1，再判断sector是否为2
lacl sector
sub #2
bcnd nosect2,NEQ
bldd taon,#CMPR1 ;sector 2
bldd tcon,#CMPR2
bldd tbon,#CMPR3
b dacout
nosect2;sector不为2，在判断sector 是否为3
lacl sector
sub #3
bcnd nosect3,NEQ
bldd taon,#CMPR1 ;sector 3
bldd tbon,#CMPR2
bldd tcon,#CMPR3
b dacout
nosect3;sector不为3，再判断sector是否为4
lacl sector
sub #4
bcnd nosect4,NEQ
bldd tcon,#CMPR1 ;sector 4
bldd tbon,#CMPR2
bldd taon,#CMPR3
b dacout
nosect4   ;sector不为4，再判断sector 是否为5
lacl sector
sub #5
bcnd nosect5,NEQ
bldd tcon,#CMPR1 ;sector 5
bldd taon,#CMPR2
bldd tbon,#CMPR3
b dacout
nosect5;若sector不为5，则必然为6
bldd tbon,#CMPR1 ;sector 6
bldd tcon,#CMPR2
bldd taon,#CMPR3
*** END sector switching
***********************************************
* END SPACE VECTOR Pulse Width Modulation
***********************************************
dacout
******************************************************
* DAC output of channels 'da1','da2','da3','da4'
* Output on 12 bit Digital analog Converter
* 5V equivalent to FFFh
******************************************************
lacc sector,7 ;scale sector by 2^7 to have good displaying
sacl sector
*** DAC out channel 'da1'
lacc #ia ;get the address of the first elements
add da1 ;add the selected output variable
;offset 'da1' sent by the terminal
sacl daout ;now daout contains the address of
;the variable to send to DAC1
lar ar5,daout ;store it in AR5
lacc * ;indirect addressing, load the value to send out
;the following 3 instructions are
;required to adapt the numeric
;format to the DAC resolution
sfr ;we have 10 bit DAC, we want to
;have the number 2000h = 5 Volt
sfr
add #800h
sacl daouttmp ;to prepare the triggering of DAC1 buffer
out daouttmp,DAC0_VAL
*** END DAC out channel 'da1'
*** DAC out channel 'da2'
lacc #ia ;get the address of the first elements
add da2 ;add the selected output variable
;offset 'da1' sent by the terminal
sacl daout ;now daout contains the address of
;the variable to send to DAC1
lar ar5,daout ;store it in AR5
lacc * ;indirect addressing, load the
;value to send out
;the following 3 instructions are
;required to adapt the numeric
;format to the DAC resolution
sfr ;we have 10 bit DAC, we want to
;have the number 2000h = 5 Volt
sfr
add #800h
sacl daouttmp ;to prepare the triggering of DAC1 buffer
out daouttmp,DAC1_VAL
*** END DAC out channel 'da2'
*** DAC out channel 'da3'
lacc #ia ;get the address of the first elements
add da3 ;add the selected output variable
;offset 'da1' sent by the terminal
sacl daout ;now daout contains the address of
;the variable to send to DAC1
lar ar5,daout ;store it in AR5
lacc * ;indirect addressing, load the value to send out
;the following 3 instructions are
;required to adapt the numeric
;format to the DAC resolution
sfr ;we have 10 bit DAC, we want to have
;the number 2000h = 5 Volt
sfr
add #800h
sacl daouttmp ;to prepare the triggering of DAC1 buffer
out daouttmp,DAC2_VAL
*** END DAC out channel 'da3'
*** DAC out channel 'da4'
lacc #ia ;get the address of the first elements
add da4 ;add the selected output variable
;offset 'da1' sent by the terminal
sacl daout ;now daout contains the address of
;the variable to send to DAC1
lar ar5,daout ;store it in AR5
lacc * ;indirect addressing, load the value to send out
;the following 3 instructions are
;required to adapt the numeric
;format to the DAC resolution
sfr ;we have 10 bit DAC, we want to have
;the number 2000h = 5 Volt
sfr
add #800h
sacl daouttmp ;to prepare the triggering of DAC1 buffer
out daouttmp,DAC3_VAL
*** END DAC out channel 'da4'
OUT tmp,DAC_VAL ;start convertion
ldp #IFRA>>7
splk #200h,IFRA ;Clear all flags, may be
;change with only T1 underflow int.
**************************************
* Context restore and Return
**************************************
larp ar7
mar *+
lacl *+ ;Accu. restored for context restore
add *+,16
lst #0,*+
lst #1,*+
clrc INTM
ret
***************************************
* END Context Restore and Return
***************************************
*************************************************************
* END _c_int2 ISR
* synchronization of the control algorithm with the PWM
* underflow interrupt
*************************************************************
_c_int0:
******************************
* Board general settings
******************************
clrc xf
**********************************************
* Function to disable the watchdog timer，初始化，不使能看门狗
**********************************************
ldp #DP_PF1
splk #006Fh, WD_CNTL