📄 pmsm3_1.asm.txt
字号:
;************************************************************
; File Name : pmsm3_1.asm
; Description : Field-Oriented Control of Three-Phase
; Permanent-Magnet Synchronous Motor
****************************************************
* Define Peripheral Registers
****************************************************
.include "x24x_app.h"
*********************************************
* External References/Prototype Declarations
*********************************************
.include "ext_refs.h"
******************************************
* Select PWM/T1 Period
******************************************
PWM_PERIOD .set 100 ; PWM period in uS (20KHz)
.if x240 | x243
T1PER_ .set PWM_PERIOD*10 ; *1000nS/(2*50nS)
.endif
.if x2407
T1PER_ .set PWM_PERIOD*15 ; *1000nS/(2*33nS)
.endif
;***********************************************
; Encoder Calibration Angle
;***********************************************
; 1000 line encoder
; 4000 counts per mechanical revolution
; 2000 counts per electrical revolution (4-pole motor)
; k*2000 corresponds to 360 degrees, k = ..., -2,-1,0,1,2,3,...
; T2CNT = 2442 on rotor lock at 0 degree
CAL_ANGLE_ .set -2442
******************************************
* Variables
******************************************
temp0 .usect "pmsmsmo",1 ; scratch pad
v_timer .usect "pmsmsmo",1 ; Virtual counter/timer
pdpint_flg .usect "pmsmsmo",1 ; PDPINT flag
phanti_flg .usect "pmsmsmo",1 ; phantom int flag
enable_flg .usect "pmsmsmo",1 ; Operation enable flag
lockrt_flg .usect "pmsmsmo",1 ; Enable lock of rotor flag
spd_lp_flg .usect "pmsmsmo",1 ; Speed loop flag
foc_flg .usect "pmsmsmo",1 ; flag to enable FOC
smo_flg .usect "pmsmsmo",1 ; flag to enable SMO based FOC
lockcnt .usect "pmsmsmo",1 ; Lock rotor counter
rampcnt .usect "pmsmsmo",1 ; Initial ramp counter
vconcnt .usect "pmsmsmo",1 ; Voltage control mode counter
stack .usect "pmsmsmo",32 ; Reserve space for stack
******************************************
* Set speed
******************************************
spd_ref_ .set 2000h ; Set speed to 1/4 of norminal
******************************************
* Initialization
******************************************
.text
.def _c_int0
_c_int0 DINT ; Set global interrupt mask on reset
Cfg_clock .if x243
Ldp #SYSCR>>7 ;
SPLK #40C0h,SYSCR ; CLKOUT=CPUCLK
.endif
.if x240
ldp #SYSCR>>7 ;
SPLK #40C0h,SYSCR ; CLKOUT=CPUCLK
splk #0020h,SYSSR ; Clear all SYSSR bits except
；SYSSR[5]
SPLK #00B1h,CKCR1 ; Configure PLL (CPUCLK=2*CLKIN)
SPLK #0001h,CKCR0 ; Disable PLL
SPLK #00C1h,CKCR0 ; Re-enable PLL
.endif
.if x2407
ldp #SCSR1>>7
splk #0284h,SCSR1 ; CLKOUT=CPUCLK=2*CLKIN, enable EVA
；& ADC. Clear SCSR[0]. SCSR[0]
；(wrongly) resets to 1
.endif
Cfg_wsgr
.if x243|x2407
LDP #temp0 ; Configure WSGR
SPLK #40H,temp0 ;
OUT temp0,0ffffh ;
.endif
.if x240
ldp #temp0 ;
SPLK #8h,temp0 ; temp0<=0008h (0w for DS&PS, 1w for IS)
OUT temp0,0ffffh ; WSGR <= (temp0)
.endif
Reset_wd0
ldp #WDKEY>>7 ; Reset WD timer
splk #wd_rst_1,WDKEY ;
splk #wd_rst_2,WDKEY ;
splk #01101111b, WDCR; disable WD
Setup_stack
LAR AR1,#stack ; Init s/w stack pointer
MAR *,AR1 ; Set up software stack
EINT
ConfigInts
ldp #EVIFRA>>7 ; Configure interrupts
splk #0fffH,EVIFRA ; Clear pending flags
splk #0ffH,EVIFRB
splk #0fH,EVIFRC
SPLK #001000000001b,EVIMRA ; Enable T1UF+PDPINT Int (PWM)
SPLK #0100b,EVIMRC ; Enable CAP3 int (QEP index)
ldp #IFR>>7
SPLK #03Fh,IFR ; Clear any pending flags
lacc IMR ;
or #00001011b ; En Int lvl 1,2,4
；(PDPINT+T1UF+Cap3)
sacl IMR ;
ldp #pdpint_flg ;
splk #0,pdpint_flg ; Clear PDPINT flag
splk #0,phanti_flg ; Clear phantom int flag
call drive_init ; Config IO for drive enable/disable
.if real_time ; Wait for manual enable
ldp #enable_flg
splk #0,enable_flg
Wait_enable
lacc enable_flg
bcnd Wait_enable,EQ
.endif
call mloop_init ; Init control loop according to
；build option
call en_drive ; Enable drive
*** END Initialization
*******************************************************
* Background Loop
*******************************************************
bloop
nop
; Code running in background
nop
Reset_wd1
ldp #WDKEY>>7 ;
splk #wd_rst_1,WDKEY ;
splk #wd_rst_2,WDKEY ;
B bloop
*** END Background Loop
.if x243|x2407
*******************************************************
* INT1 - PDP & Sys Peripheral Ints
* Enabled: PDPINT
*******************************************************
PDPINT_ID .set 020h ; PDPINT vector ID
PDPINT_CLR .set 01h ; PDPINT flag clear
.def _c_int1 ; int1-PDP Sys Pers
；dispatcher/service
_c_int1
MAR *,AR1 ; Save context
mar *+ ; point to a guaranteed unused
；location
SST #1, *+ ; save ST1
SST #0, *+ ; save ST0
SACH *+ ; save acc high
SACL *+ ; save acc low and point at an unused
；loca.More context save if needed
setc SXM ; set sign extension mode
clrc OVM ; clear overflow mode
ldp #PIVR>>7 ; set DP
LACC PIVR ; load peripheral int
；vector/ID/offset
SUB #PDPINT_ID ; PDPINT?
bcnd Pdp_int,EQ ; to PDPISR if zero
call phantom ; got a phantom int if not
b rest_int1 ; return
Pdp_int
Ldp #EVIFRA>>7 ;
splk #PDPINT_CLR,EVIFRA ; Clear PDPINT flag
ldp #pdpint_flg ; set DP
splk #1,pdpint_flg ; set flag
rest_int1 ;More context restore if needed
MAR *, AR1 ; make stack pointer active
MAR *- ; point to top of stack
LACL *- ; Restore Acc low
add *-,16 ; Restore Acc high
LST #0, *- ; restore ST0
LST #1, *- ; restore ST1 and pointer
EINT ;
RET ; return
*** END INT1 - PDP & Sys Peripheral Ints
.endif
*******************************************************
* INT2 - EV group A Ints
* Enabled: T1UF, ...
*******************************************************
T1PR_ID .set 027h ; GPT1 Period interrupt offset/ID
T1UF_ID .set 029h ; GPT1 underflow interrupt offset/ID
T1PR_CLR .set 080h ; GPT1 period interrupt flag clear
T1UF_CLR .set 0200h ; GPT1 underflow interrupt flag clear
.if x240
PDPINT_ID
.set 020H ; PDPINT vector ID
.endif
.def _c_int2 ; int2-EV group A dispatcher/service
_c_int2
MAR *,AR1 ; Save context
Mar *+ ; point to a guaranteed unused location
SST #1, *+ ; save ST1
SST #0, *+ ; save ST0
SACH *+ ; save acc high
SACL *+ ; save acc low
Sar AR2,*+ ; save AR2 (used by atan_div)
popd *+ ; save TOS to free h/w stack
;More context save if needed
setc SXM ; set sign extension mode
clrc OVM ; clear overflow mode
.if x243 | x2407
ldp #PIVR>>7 ; set DP
LACC PIVR ; load per int vector/ID/offset
SUB #T1PR_ID ; GPT1 PR INT?
.endif
.if x240
ldp #EVIVRA>>7
lacc EVIVRA ; load int vector ID
sub #PDPINT_ID ; PDPINT?
bcnd No_pdp,NEQ ; continue if no
Pdp_is
ldp #pdpint_flg ; Set PDPINT flag if yes
splk #1,pdpint_flg ;
b rest_int2
No_pdp
SUB #T1PR_ID-PDPINT_ID ; GPT1 PR INT?
.endif
BCND Not_pr_int,NEQ ; branch if not
Pr_is
ldp #EVIFRA>>7 ; yes, got GPT1 pr INT
.if x243 | x2407
ldp #EVIFRA>>7 ; yes, got GPT1 pr INT
splk #T1PR_CLR,EVIFRA ; clear GPT1 PR INT flag
.endif
;call routines running on PRINT
b rest_int2 ; return
Not_pr_int
SUB #T1UF_ID-T1PR_ID ; GPT1 UF INT?
bcnd Uf_is,EQ ; GPT1 uf int?
Not_uf_int
call phantom ; got a phantom int if not
b rest_int2 ; return
Uf_is
.if x243 | x2407
ldp #EVIFRA>>7 ; yes, got GPT1 UF INT
splk #T1UF_CLR,EVIFRA ; clear GPT1 UF INT flag
.endif
Exe_mloop call main_loop ; Execute main control loop
Update_vt
ldp #v_timer ;
LACC v_timer ; Update virtual timer
ADD #1 ;
SACL v_timer ; Save it
rest_int2
MAR *, AR1 ; make stack pointer active
MAR *- ; point to top of stack
;More context restore if needed
pshd *- ; restore TOS
lar AR6,*- ; restore AR6
lar AR2,*- ; restore AR2
LACL *- ; Restore Acc low
Add *-,16 ; Restore Acc high
LST #0, *- ; restore ST0
LST #1, *- ; restore ST1 and pointer
EINT
RET
*** END INT2 - EV group A Ints
*******************************************************
* INT4 - EV group C Ints
* Enabled: Capture 3 int as QEP index
*******************************************************
Cap3_ID .set 035H ; Capture 3 int vector ID
Cap3_Clr .set 04H ; Capture 3 int flag clear
.def _c_int4 ; int4-EV group C dispatcher/service
_c_int4
MAR *,AR1 ; Save context
Mar *+ ; point to a guaranteed unused location
SST #1, *+ ; save ST1
SST #0, *+ ; save ST0
SACH *+ ; save acc high
SACL *+ ; save acc low and point to an unused
；loca. More context save if needed
.if x243 | x2407
ldp #PIVR>>7 ; set DP
LACC PIVR ; load peripheral int vector/ID/offset
.endif
.if x240
ldp #EVIVRC>>7
lacc EVIVRC
.endif
SUB #Cap3_ID ; Capture 1 int?
bcnd Cap3_int,EQ ; to PDPISR if zero
call phantom ; got a phantom int if not
b rest_int4 ; return
Cap3_int
.if x243 | x2407
ldp #EVIFRC>>7
splk #Cap3_Clr,EVIFRC ; Clear Capture 1 int flag
.endif
call QEP_INDEX_ISR_DRV ; QEP index routine
rest_int4 ;More context restore if needed
MAR *, AR1 ; make stack pointer active
MAR *- ; point to top of stack
LACL *- ; Restore Acc low
add *-,16 ; Restore Acc high
LST #0, *- ; restore ST0
LST #1, *- ; restore ST1 and pointer
EINT
RET ; return
*** END INT4 - EV group C Ints
*******************************************************
* Unused/phantom interrupts
*******************************************************
.if x240
.def _c_int1 ; int1-System module ints
.endif
.def _c_int3 ; int3-EV group B dispatcher/service
.def _c_int5 ; int5-Sys Pers dispatcher/service
.def _c_int6 ; int6-Sys Pers dispatcher/service
.def _c_nmi ; nmi service
.if x240
_c_int1 ; int1-System module interrupts
.endif
_c_int3 ; int3-EV group B dispatcher/service
_c_int5 ; int5-Sys Pers dispatcher/service
_c_int6 ; int6-Sys Pers dispatcher/service
_c_nmi ; nmi service
phantom
MAR *,AR1 ; Save context
mar *+ ; point to a guaranteed unused location
SST #1, *+ ; save ST1
SST #0, *+ ; save ST0 and point to an unused location
;More context save if needed
ldp #phanti_flg
splk #1,phanti_flg ; set phantom int flag
;More context restore if needed
MAR *, AR1 ; make stack pointer active
MAR *- ; point to top of stack
LST #0, *- ; restore ST0
LST #1, *- ; restore ST1 and pointer
EINT ;
RET ; return
*** END Unused Interrupts
**********************************************************
* Main Control Loop Initiate
**********************************************************
mloop_init
call QEP_THETA_DRV_INIT ; Init QEP driver
ldp #cal_angle ; Set encoder cal angle
splk #CAL_ANGLE_,cal_angle
call SPEED_FRQ_INIT ; Init speed calculator
CALL ILEG2DRV_INIT ; Init 2-shunt currrent sensing
CALL CLARKE_INIT ; Init Clarke routine
CALL PARK_INIT ; Init I Parke routine
ldp #theta_p ;
splk #0,theta_p ; lock rotor
call pid_reg_iq_init ; Q-axis PI current regulator
ldp #iq_ref ;
splk #0,iq_ref ; Set Q-axis current reference
call pid_reg_id_init ; D-axis PI current regulator
ldp #id_ref ;
splk #400h,id_ref ; Set D-axis current reference
CALL I_PARK_INIT ; Init inverse Parke routine
ldp #theta_ip ;
splk #0,theta_ip ; lock rotor
CALL SVGEN_DQ_INIT ; Init SV PWM routine
CALL FC_PWM_DRV_INIT ; Configure PWM outputs
ldp #n_period ; Init T1/PWM period (norminal)
splk #T1PER_,n_period ;
ldp #spd_lp_flg ;
splk #0,spd_lp_flg ; Start with open speed loop
splk #7FFFh,lockcnt ; initialize lock rotor counter
splk #1,lockrt_flg ; set lock rotor flag
ret
**********************************************************
* Main Control Loop
**********************************************************
main_loop
Lockrot
ldp #lockcnt ;
lacl lockcnt
bcnd Endlock,LEQ
sub #1
sacl lockcnt
splk #1,lockrt_flg ; set lock rotor flag
splk #0,temp0 ; set rotor angle to zero
b Fdb_path
Endlock
splk #0,lockrt_flg ; clear lock rotor flag
bldd #theta_elec,temp0 ; use rotor angle feedback
ldp #iq_ref
splk #500h,iq_ref ; set iq reference
splk #0,id_ref ; set id reference
Fdb_path
call QEP_THETA_DRV ; Feedback paths
call SPEED_FRQ ; calculate speed
CALL ILEG2DRV
ldp #clark_a
bldd #Ia_out,clark_a
bldd #Ib_out,clark_b
CALL CLARKE
ldp #park_d
bldd #clark_d,park_d
bldd #clark_q,park_q
bldd #temp0,theta_p
call PARK
ldp #spd_lp_flg ; Forward control path
lacc spd_lp_flg
bcnd Open_lp,EQ
Cl_lp
ldp #spd_fdb
bldd #speed_frq,spd_fdb
call pid_reg_spd
ldp #iq_ref
bldd #spd_out,iq_ref
Open_lp
ldp #iq_fdb
bldd #park_Q,iq_fdb
call pid_reg_iq
ldp #id_fdb
bldd #park_D,id_fdb
call pid_reg_id
ldp #ipark_Q
bldd #uq_out,ipark_Q
bldd #ud_out,ipark_D
bldd #temp0,theta_ip
CALL I_PARK
ldp #Ubeta
bldd #ipark_q,Ubeta
bldd #ipark_d,Ualfa
CALL SVGEN_DQ
ldp #Mfunc_c1
bldd #Ta,Mfunc_c1
bldd #Tb,Mfunc_c2
bldd #Tc,Mfunc_c3
CALL FC_PWM_DRV
ret
*** END
*** END Whole Code
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -