foc.asm

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

.bss Teta_cm,1 ;real rotor flux position, output
;of the current model
.bss serialtmp,1 ;serial communication temporary
;variable
.bss da1,1 ;DAC displaying table offset for DAC1
.bss da2,1 ;DAC displaying table offset for DAC2
.bss da3,1 ;DAC displaying table offset for DAC3
.bss da4,1 ;DAC displaying table offset for DAC4
.bss VDCinvT,1 ;VDCinv*(T/2) (used in SVPWM)
.bss tetaincr,1 ;variable used in current model
.bss Index,1 ;pointer used to access sine look-up table
* PI regulators variable
.bss upi,1 ;PI regulators (current and speed) output
.bss elpi,1 ;PI regulators (current and speed)
;limitation error
.bss encincr,1 ;encoder pulses increment between
;two consecutive Sampling periods
.bss speedtmp,1 ;used to accumulate encoder pulses
;increments (to calculate the
;speed each speed sampling period)
.bss speedstep,1 ;sampling periods down counter
;used to define speed sampling
;period
*** END Variables and constants initializations
.text ;link in "text section"
********************************************************* *
* _c_int2 ISR *
* synchronization of the control algorithm with the PWM *
* underflow interrupt *
********************************************************* *
_c_int2:
************************
* Context Saving
************************
larp ar7 ;context save
mar *-
sst #1,*- ;status register 1
sst #0,*- ;status register 0
sach *- ;Accu. low saved for context save
sacl *- ;Accu. high saved
* END Context Saving *
mar *,ar5 ;used later for DACs output
************************************
* Control ISR
* Description: Control Algorithm ISR
* Last Update:17 november 1997
************************************
************************************
* initialization phase
************************************
ldp #ctrl_n ;control variable page
lacc run
bcnd noinit,NEQ
lacc #0
sacl vSal_ref
sacl vSbe_ref
b init
************************************
* END initialization phase
************************************
noinit
***********************************************************
* Current sampling - AD conversions
* N.B. we will have to take only 10 bit (LSB)
***********************************************************
ldp #DP_PF1
splk #1801h,ADC_CNTL1 ;ia and ib conversion start
;ADCIN0 selected for ia A/D1
;ADCIN8 selected for ib A/D2
conversion
bit ADC_CNTL1,8;位测试，将指定位复制到状态寄存器ST1的TC位，这里的8指的是ADC_CNTL1的第7位，表示转换的状态位
bcnd conversion,tc ;wait approximatly 6us即若转换正在进行就循环，转换完成就向下进行
lacc ADC_FIFO1,10 ;10.6 format
ldp #ctrl_n ;control variable page
sach tmp;tmp为暂时存储变量的一段数据空间，被.bss定义
lacl tmp    ;现在累加器中的值是Ia
and #3ffh   ;将Ia转化为Q12格式（即and #3ffh至mpy Kcurrent等几条命令）
sub #512 ;then we have to subtract the offset (2.5V) to have
;positive and negative values of the sampled current
sacl tmp
spm 3 ;PM=11, 6 right shift after multiplication即乘之后PREG乘积寄存器输出右移6位
lt tmp
mpy Kcurrent
pac ;将乘积寄存器载入累加器
sfr;该指令为累加器算术右移指令
sfr
sacl ia ;PM=11, +2 sfr= 8 right shift
spm 0
sub #112 ;then we subtract a DC offset
;(that should be zero, but it isn't)
sacl ia ;sampled current ia, 4.12 format
ldp #DP_PF1
lacc ADC_FIFO2,10  ;将Ib采样值取到累加器
ldp #ctrl_n ;control variable page
sach tmp
lacl tmp
and #3ffh   ;将Ib转换为Q12格式（与Ia转化步骤一样）
sub #512
sacl tmp
spm 3
lt tmp
mpy Kcurrent
pac
sfr
sfr ;PM=11, +2 sfr= 8 right shift
add #-80 ;then we subtract a DC offset
;(that should be zero, but it isn't)
sacl ib
spm 0 ;PM=00
***********************************************************
* END Current sampling - AD conversions
***********************************************************
*********************************************
* Clarke transformation
* (a,b) -> (alfa,beta)
* iSalfa = ia*sqrt(3/2)
* iSbeta = (2 * ib + ia) * sqrt(2)/2
*********************************************
lt ia
mpy #5018 
pac  ;将乘积寄存器的值载入累加器
sach iSalfa,4;iSalfa 4.12 format
lacc ib,1;将ib*2放入累加器（左移一位就是乘以2）
acc  ia
sacl tmp
lt tmp
mpy #2896
pac
sach iSbeta,4 ;iSbeta 4.12 format
**********************************
* END Clarke transformation
**********************************
**************************************
* Measured speed and control
**************************************
*** encoder pulses reading
ldp #DP_EV
lacc T3CNT ;we read the encoder pulses
splk #0000h,T3CNT
ldp #ctrl_n ;control variable page
sacl encincr
*** END Encoder pulses reading
*******************************************************
* Calculate speed and update reference speed variables
*******************************************************
lacc speedstep ;are we in speed control loop
;(SPEEDSTEP times current control loop)
sub #1
sacl speedstep;speedstep是速度采样周期减计数器，速度的采样周期要比电流采样周期长
bcnd nocalc,GT ;if we aren't, skip speed calculation即如果ACC>0也就是没到速度采样时间，就转到nocalc去执行，否则向下执行
**********************************************
* Speed calculation from encoder pulses
**********************************************
spm 3 ;PM=11, 6 right shift after multiplication
lt speedtmp ;multiply encoder pulses by Kspeed
;(8.8 format constant)
;to have the value of speed
mpy #Kspeed
pac
sfr
sfr ;PM=11, +2 sfr= 8 right shift
sacl n
lacc #0 ;zero speedtmp for next calculation
sacl speedtmp
lacc #SPEEDSTEP ;restore speedstep to the value
;SPEEDSTEP
sacl speedstep ;for next speed control loop
spm 0 ;PM=00, no shift after multiplication
**********************************************
* END Speed calculation from encoder pulses
**********************************************
*****************************************************
* Speed regulator with integral component correction(带积分修正的PI速度调节器）
*****************************************************
lacc n_ref
sub n
sacl epin ;epin=n_ref-n, 4.12 format
lacc xin,12
lt epin
mpy Kpin
apac      ;PI调节器的输出UPI=epin(速度偏差)*Kpin(比例系数)+xin(转速调节器积分累积量)
sach upi,4 ;upi=xin+epin*Kpin, 4.12 format  
;here we start to saturate
bit upi,0
bcnd upimagzeros,NTC ;If value >0 we branch
lacc #Isqrefmin ;negative saturation
sub upi
bcnd neg_sat,GT ;if upi<ISqrefmin then branch to saturate
lacc upi ;value of upi is valid
b limiters
neg_sat
lacc #Isqrefmin ;set acc to -ve saturated value
b limiters
upimagzeros ;Value is positive
lacc #Isqrefmax ;positive saturation
sub upi
bcnd pos_sat,LT ;if upi>ISqrefmax then branch to saturate
lacc upi ;value of upi valid
b limiters
pos_sat
lacc #Isqrefmax ;set acc to +ve saturated value
limiters
sacl iSqref ;Store the acc as reference value
sub upi
sacl elpi ;elpi=iSqref-upi, 4.12 format
lt elpi ;if there is no saturation elpi=0
mpy Kcorn
pac
lt epin
mpy Kin
apac     ;乘积寄存器加至累加器，pac是乘积寄存器载入累加器
add xin,12
sach xin,4 ;xin=xin+epin*Kin+elpi*Kcorn, 4.12 format其中elpi为PI调节器极限偏差，elpi=ITREF(电流参考值即PI调节器最终的输出）- UPI(可能超过限幅值，取ITREFMAX或ITREFMIN，若没超取UPI)
***********************************************************
* END Speed regulator with integral component correction
***********************************************************
nocalc ;branch here if we don't have to calculate the speed
lacc speedtmp ;use the actual encoder increment to ;update the
;increments accumulator used to calculate the speed
add encincr
sacl speedtmp
**************************************
* END Measured speed and control
**************************************
********************************************
* sinTeta_cm, cosTeta_cm calculation（根据Teta_cm查sin cos表）
********************************************
mar *,ar5
lt Teta_cm ;current model rotor flux position
mpyu SR8BIT  ;mpyu为无符号乘法，即Teta_cm*100h，相当于左移8位
pac     ;乘积寄存器载入累加器
sach Index
lacl Index;生成查表指针
and #0ffh;屏蔽高位
add #sintab  ;加上正弦表的首地址
sacl tmp
lar ar5,tmp
lacl *
sacl sinTeta_cm ;sine Teta_cm value, 4.12 format保存正弦值
lacl Index ;The same for Cos ...
;cos(teta)=sin(teta+90度)
add #40h ;90度 = 40h elements of the table
and #0ffh
add #sintab    ;加上正弦表的首地址
sacl tmp
lar ar5,tmp
lacc *
sacl cosTeta_cm ;cosine Teta_cm value, 4.12 format保存余弦值
********************************************
* END sinTeta_cm, cosTeta_cm calculation
********************************************
***********************************************
* Park transformation
* (alfa, beta)->(d,q)
* iSd=iSalfa*cos(Teta_cm)+iSbeta*sin(Teta_cm)
* iSq=-iSalfa*sin(Teta_cm)+iSbeta*cos(Teta_cm)
***********************************************
lacc #0
lt iSbeta    
mpy sinTeta_cm;preg寄存器中为iSbeta*sin
lta iSalfa    ;lta表示将iSalfa放入T（临时）寄存器后，再将preg内容加至累加器ACC
mpy cosTeta_cm;现在preg内容为iSalfa*cos
mpya sinTeta_cm;mpya表示乘至preg后，将上次preg中的值加至累加器ACC，即现在ACC中为iSbeta*sin+iSalfa*cos，而preg中为iSalfa*sin
sach iSd,4 ;iSd 4.12 format
lacc #0
lt iSbeta
mpys cosTeta_cm;现在preg为iSbeta*cos,而ACC中为0-iSalfa*sin=-iSalfa*sin
apac;表示将preg中的内容加至累加器，即现在ACC中的内容为-iSalfa*sin+iSbeta*cos
sach iSq,4 ;iSq 4.12 format
**********************************
* END Park transformation
**********************************
***********************************************
* Current Model（下面为转子磁链位置的计算，语句基本与2407的一致）
***********************************************
lacc iSd       ;iSd（I的d轴分量）即2407程序中IM
sub i_mr       ;转子励磁电流，即2407原程序中的IDK
sacl tmp
lt tmp
mpy #Kr        ;Kr=0eh
pac
sach tmp,4
lacc tmp
add i_mr
sacl i_mr ;i_mr=i_mr+Kr*(iSd-i_mr), 4.12 f即Q12格式
bcnd i_mrnotzero,NEQ;如果i_mr非零则跳转
lacc #0
sacl tmp ;if i_mr=0 then tmp=iSq/i_mr=0
b i_mrzero
i_mrnotzero
*** division (iSq/i_mr)
lacc i_mr
bcnd i_mrzero,EQ
sacl tmp1
lacc iSq
abs
sacl tmp
lacc tmp,12
rpt #15
subc tmp1
sacl tmp ;tmp=iSq/i_mr
lacc iSq
bcnd iSqpos,GT
lacc tmp
neg
sacl tmp ;tmp=iSq/i_mr, 4.12 format
iSqpos
i_mrzero
*** END division ***
lt tmp
mpy #Kt
pac
sach tmp,4 ;slip frequency, 4.12 format
lacc tmp ;load tmp in low ACC
add n       ;与2407源程序相比少了一句:"SFR".
sacl fs ;rotor flux speed, 4.12 format,
;fs=n+Kt*(iSq/i_mr)
*** rotor flux position calculation 转子磁链位置的计算***
lacc fs  ;fs是转子磁链的速度
abs
sacl tmp
lt tmp
mpy #K
pac
sach tetaincr,4
bit fs,0
bcnd fs_neg,TC
lacl tetaincr
adds Teta_cm
sacl Teta_cm
b fs_pos
fs_neg;fs为负
lacl Teta_cm
subs tetaincr
sacl Teta_cm
;Teta_cm=Teta_cm+K*fs=Teta_cm+tetaincr
;(0;360)<->(0;65535)
fs_pos;fs为正
rpt #3
sfr    ;右移4位，相当于左移12位，表示除24，变成0-4096范围（可以用一句"LACC  Teta_cm,12"来替代）
sacl Teta_cm1 ;(0;360)<->(0;4096), this variable
;is used only for the visualization
***********************************************
* END Current Model
***********************************************
*****************************************************
* Field Weakening function
* input:n_ref, output iSdref 4.12 format
*****************************************************
spm 2 ;PM=10, four left shift after multiplication
lacc n_ref
abs ;we consider absolute value of speed reference
rpt #3
sfr
sacl n_ref8_8 ;speed reference 8.8f
sub #100h
bcnd noFieldWeakening,LEQ
lacc p0,12
lt n_ref8_8
mpy p1
apac
sach tmp,4 ;tmp=p0+p1*n_ref
sqra n_ref8_8
pac
sach tmp1,4
lacc tmp,12
lt tmp1 ;tmp1=n_ref^2
mpy p2
apac
sach tmp,4 ;tmp=p0+p1*n_ref+p2*(n_ref^2)
lt tmp1
mpy n_ref8_8
pac
sach tmp1,4 ;tmp1=n_ref^3
lacc tmp,12
lt tmp1
mpy p3
apac
sach tmp,4 ;tmp=p0+p1*n_ref+p2*(n_ref^2)+p3*(n_ref^3)
lacc tmp,4 ;iSdref 8.8 f
sacl iSdref ;iSdref 4.12 f with Field Weakening
b endFW
noFieldWeakening
lacc #2458 ;iSdref=0.6 pu
sacl iSdref ;iSdref 4.12 f without Field Weakening
endFW
spm 0 ;PM=0
*****************************************************
* END Field Weakening function
*****************************************************
**********************************************************
* q-axis current regulator with integral component * correction
* (iSq,iSqref)->(vSqref)
;以下这一小节的代码和2407的是一样的，不需要改动
**********************************************************
lacc iSqref   ;q轴为交轴，相当于T轴，所以iSqref=2407中的ITREF
sub iSq
sacl epiq ;epiq=iSqref-iSq, 4.12 format
lacc xiq,12
lt epiq
mpy Kpi
apac
sach upi,4 ;upi=xiq+epiq*Kpi, 4.12 format
bit upi,0
bcnd upimagzeroq,NTC
lacc #Vmin
sub upi
bcnd neg_satq,GT ;if upi<Vmin branch to saturate
lacc upi ;value of upi is valid
b limiterq
neg_satq
lacc #Vmin ;set ACC to neg saturation
b limiterq
upimagzeroq ;Value was positive
lacc #Vmax
sub upi
bcnd pos_satq,LT ;if upi>Vmax branch to saturate
lacc upi ;value of upi is valid