focpmsm.asm

是关于交流电机、无刷电机、有刷电机、步进`电机等不同电机的dsp汇编代码

;already have the right value in teta_m
bcnd encminmax,LT ;
sacl teta_m ;otherwise the value of teta_m is greater
;than Encpulses and so we have to store the
encminmax ;right value ok, now teta_m contains the
; right value in the range
lacc tmp ;[0,Encpulses-1]
;the actual value will be the old one during
; the next sampling period
sacl encoderold
.endif
.if position_scaling
*******************
* Teta calculation
*******************
lt teta_m ;multiply teta_m pulses by Kencoder (4.12
;format constant) to have the rotor
;electrical position
mpyu Kencoder ;encoder pulses = 0 -> teta = 0fffh = 0 degrees
pac ;encoder pulses = 1600 -> teta = 1fffh = 1*360
;encoder pulses = 3200 -> teta = 2fffh = 2*360
and #0fffh
sacl teta_e
.endif
.if speed_scaling
*******************************************************
* Calculate speed and update reference speed variables
*******************************************************
lacc speedstep ;are we in speed control loop ? (SPEEDSTEP
;times current control loop)
sub #1 ;
sacl speedstep ;
bcnd nocalc,GT ;if we aren't, skip speed calculation
*** Speed calculation from encoder pulses
lt speedtmp ;multiply encoder pulses by Kspeed (8.8
; format constant)
;to have the value of speed
mpy #Kspeed ;
pac ;
rpt #7 ;
sfr ;
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
.endif
.if speed_regulator
*****************************************************
* Speed regulator with integral component correction
*****************************************************
lacc n_ref
sub n
sacl epispeed
lacc xispeed,12
lt epispeed
mpy Kpispeed
apac
sach upi,4
;here start to saturate
bit upi,0
bcnd upimagzeros,NTC ;If value +ve branch
lacc iSqrefmin
sub upi
bcnd neg_sat,GT ;if upi<iqrmin 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
sub upi ;
bcnd pos_sat,LT ;if upi>iqrmax 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
lt elpi
mpy Kcorspeed
pac
lt epispeed
mpy Kispeed
apac
add xispeed,12
sach xispeed,4
.endif
.if speed_scaling
****************************************************
* Encoder update
****************************************************
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 ;
.endif
Init
.if current_scaling
*********************************************
* Sampled current scaling
* to nominal current 1000h <-> I_nominal
*********************************************
ldp #ia
lacc ia
and #3ffh
sub #440 ;then we have to subtract the offset (2.5V) to
; have positive and negative values of the
; sampled current
sacl tmp
spm 3
lt tmp
mpy Kcurrent
pac
sfr
sfr
sacl ia ;sampled current ia, f 4.12
lacc ib
and #3ffh
sub #440
sacl tmp
lt tmp
mpy Kcurrent
pac
sfr
sfr
sacl ib
add ia
neg
sacl ic ;ic = -(ib+ia)
spm 0
.endif
.if clarke
*********************************************
* (a,b,c) -> (alfa,beta) axis transformation
* iSalfa = ia
* iSbeta = (2 * ib + ia) / sqrt(3)
*********************************************
lacc ia
sacl iSalfa
lacc ib,1 ;iSbeta = (2 * ib + ia) / sqrt(3)
add ia
sacl tmp
lt tmp
mpy SQRT3inv ;SQRT3inv = (1 / sqrt(3)) = 093dh
;4.12 format = 0.577350269
pac
sach iSbeta,4
.endif
.if sine_table
*********************************************
* Sine and cosine wave calculation from
* teta values using sine look-up table
*********************************************
lacc teta_e ;teta range is [0;1000h] 4.12 format = [0;360]
;so we have a pointer (in the range [0;0ffh])
;to the sine look-up table in the second and
;third nibble
rpt #3
sfr
and #0ffh ;now ACC contains the pointer to access the table
sacl index
add #sintab
sacl tmp
lar ar5,tmp
nop ; prevent pipeline conflict
nop
mar *,ar5
lacl *
nop
sacl sin ;now we have sine value
lacl index ;the same thing for cosine ... cos(teta)
;sin(teta+90°)
add #040h ;90 degrees = 40h elements of the table
and #0ffh
sacl index ;we use the same pointer (we don't care)
add #sintab
sacl tmp
lar ar5,tmp
lacc *
sacl cos ;now we have cosine value
.endif
.if park
*********************************************
* d-axis and q-axis current calculation
* (alfa,beta) -> (d,q) axis transformation
* iSd = iSalfa * cos(teta_e) + iSbeta * sin(teta_e)
* iSq =-iSalfa * sin(teta_e) + iSbeta * cos(teta_e)
*********************************************
lacc #0
lt iSbeta ;TREG0=iSbeta
mpy sin ;PREG=iSbeta*sin(teta_e)
lta iSalfa ;ACC+=PREG ; TREG0=iSalfa
mpy cos ;PREG=iSalfa*cos(teta_e)
mpya sin ;ACC+=PREG ; PREG=iSalfa*sin(teta_e)
sach iSd,4
lacc #0 ;ACC=0
lt iSbeta ;TREG0=ibeta
mpys cos ;ACC-=(PREG=iSalfa*sin(teta_e))
apac ;ACC+=PREG
sach iSq,4
.endif
.if isq_regulator
**************************************************************
* q-axis current regulator with integral component correction
* (iSq,iSqref)->(vSqref)
**************************************************************
iq_reg:
lacc iSqref
sub iSq
sacl epiq
lacc xiq,12
lt epiq
mpy Kpi
apac
sach upi,4
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
b limiterq
pos_satq
lacc #Vmax ;set ACC to pos saturation
limiterq
sacl vSqref ;Save ACC as reference value
sub upi
sacl elpi
lt elpi
mpy Kcor
pac
lt epiq
mpy Ki
apac
add xiq,12
sach xiq,4
.endif
.if isd_regulator
**************************************************************
* d-axis current regulator with integral component correction
* (iSd,iSdref)->(vSdref)
**************************************************************
lacc iSdref
sub iSd
sacl epid
lacc xid,12
lt epid
mpy Kpi
apac
sach upi,4
bit upi,0
bcnd upimagzerod,NTC
lacc #Vmin
sub upi
bcnd neg_satd,GT ;if upi<Vmin branch to saturate
lacc upi ;value of upi is 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 ;value of upi is valid
b limiterd
pos_satd
lacc #Vmax ;set ACC to pos saturation
limiterd
sacl vSdref ;Save ACC as reference value
sub upi
sacl elpi
lt elpi
mpy Kcor
pac
lt epid
mpy Ki
apac
add xid,12
sach xid,4
.endif
.if inv_park
***********************************************
* alfa-axis and beta-axis voltages calculation
* (d,q) -> (alfa,beta) axis transformation
* vSbe_ref = vSqref * cos(teta_e) + vSdref * sin(teta_e)
* vSal_ref =-vSqref * sin(teta_e) + vSdref * cos(teta_e)
***********************************************
lacc #0
lt vSdref ;TREG0=vSdref
mpy sin ;PREG=vSdref*sin(teta_e)
lta vSqref ;ACC+=PREG ; TREG0=vSqref
mpy cos ;PREG=vSqref*cos(teta_e)
mpya sin ;ACC+=PREG ; PREG=vSqref*sin(teta_e)
sach vSbe_ref,4
lacc #0 ;ACC=0
lt vSdref ;TREG0=vSdref
mpys cos ;ACC-=(PREG=vSqref*sin(teta_e))
apac ;ACC+=PREG
sach vSal_ref,4
.endif
.if svpwm
**********************************************
* Phase 1(=a) 2(=b) 3(=c) Voltage calculation
* (alfa,beta) -> (a,b,c) axis transformation
* modified exchanging alfa axis with beta axis
* for a correct sector calculation in SVPWM
* Va = vSbe_ref
* Vb = (-vSbe_ref + sqrt(3) * vSal_ref) / 2
* Vc = (-vSbe_ref - sqrt(3) * vSal_ref) / 2
***********************************************
lt vSal_ref ;TREG0=vSal_ref
mpy SQRT32 ;PREG=vSal_ref*(SQRT(3)/2)
pac ;ACC=PREG
sub vSbe_ref,11 ;ACC-=vSbe_ref*2^11
sach Vb,4
pac ;ACC=PREG
neg ;ACC=-ACC
sub vSbe_ref,11 ;ACC-=vSbe_ref*2^11
sach Vc,4
lacl vSbe_ref ;ACC=vSbe_ref
sacl Va ;Va=ACCL
**************************************
* SPACE VECTOR Pulse Width Modulation
* (see SVPWM references)
**************************************
lt VDCinvT
mpy SQRT32
pac
sach tmp,4
lt tmp
mpy vSbe_ref
pac
sach X,4
lacc X ;ACC = vSbe_ref*K1
sach accb
sacl accb+1 ;ACCB = vSbe_ref*K1
sacl X,1 ;X=2*vSbe_ref*K1
lt VDCinvT
splk #1800h,tmp
mpy tmp ;implement mpy #01800h
pac
sach tmp,4
lt tmp
mpy vSal_ref
pac
sach tmp,4
lacc tmp ;reload ACC with vSal_ref*K2
add accb+1
add accb,16
sacl Y ;Y = K1 * vSbe_ref + K2 * vSal_ref
sub tmp,1
sacl Z ;Z = K1 * vSbe_ref - K2 * vSal_ref
*** 60 degrees sector determination
lacl #0
sacl sector
lacc Va
bcnd Va_neg,LEQ ;If Va<0 do not set bit 1 of sector
lacc sector
or #1
sacl sector ;implement opl #1,sector
Va_neg lacc Vb
bcnd Vb_neg,LEQ ;If Vb<0 do not set bit 2 of sector
lacc sector
or #2
sacl sector ;implement opl #2,sector
Vb_neg lacc Vc
bcnd Vc_neg,LEQ ;If Vc<0 do not set bit 3 of sector
lacc sector
or #4
sacl sector ;implement opl #4,sector
Vc_neg
*** END 60 degrees sector determination
*** T1 and T2 (= t1 and t2) calculation depending on the sector number
lacl sector ;(see SPACE VECTOR Modulation references for
;details)
sub #1
bcnd no1,NEQ
lacc Z
sacl t1
lacc Y
sacl t2
b t1t2out
no1
lacl sector
sub #2
bcnd no2,NEQ
lacc Y
sacl t1
lacc X
neg
sacl t2
b t1t2out
no2
lacl sector
sub #3
bcnd no3,NEQ
lacc Z
neg
sacl t1
lacc X
sacl t2
b t1t2out
no3
lacl sector
sub #4
bcnd no4,NEQ
lacc X
neg
sacl t1
lacc Z
sacl t2
b t1t2out
no4
lacl sector
sub #5
bcnd no5,NEQ
lacc X
sacl t1
lacc Y
neg
sacl t2
b t1t2out
no5
lacc Y
neg
sacl t1
lacc Z
neg
sacl t2
t1t2out
lacc t1 ;t1 and t2 minumum values must be Tonmax