📄 pmsm3_2.asm
字号:
;************************************************************
; File Name: pmsm3_2.asm
; Originator: Digital Control Systems Group
; Texas Instruments
; Description: Sensorless Field-Oriented Control of
; Three-Phase Permanent-Magnet Synchronous Motor
; Target: 'x24x/x240x family processors
; Spectrum Digital DMC1500/1000 motor drive platforms
; Sinusoidal Type Permanent-Magnet Synchronous Motor
;___________________________________________________________
; Date of Mod | DESCRIPTION
; ------------|---------------------------------------------
; Sept. 15, 00| Released as version 1.0
; |
; ------------|---------------------------------------------
****************************************************
* Define Peripheral Registers
****************************************************
.include "x24x_app.h"
*********************************************
* External References/Prototype Declarations
*********************************************
.include "ext_refs.h"
******************************************
* Select Debug Mode
******************************************
real_time .set 1 ; set to 1 for r/t mode, o/w set 0
******************************************
* Select PWM/T1 Period
******************************************
PWM_PERIOD .set 100 ; PWM period in uS (10KHz)
.if x240 | x243
T1PER_ .set PWM_PERIOD*10 ; *1000nS/(2*50nS)
.endif
.if x2407
T1PER_ .set PWM_PERIOD*20 ; *1000nS/(2*25nS) for 40MHz CLK
;T1PER_ .set PWM_PERIOD*15 ; *1000nS/(2*33nS) for 30MHz CLK
.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
;***********************************************
; Select Incremental Build of Main Control Loop
; One at a time !
;***********************************************
I_build1 .set 1 ; Code framework and forward control path
I_build2 .set 0 ; Current sensing and feedback path
I_build3 .set 0 ; D&Q current loops and regulators
I_build4 .set 0 ; Encoder i/f driver and calibration
I_build5 .set 0 ; Current mode FOC and current regulators
I_build6 .set 0 ; Speed calculator and speed loop
; (I_build6 to be completed in future)
I_build7 .set 0 ; SMO rotor angle estimator and SMO based FOC
I_build8 .set 0 ; Startup sequence
I_build9 .set 0 ; SMO rotor speed estimator and speed loop
; (I_build9 to be completed in the future)
******************************************
* 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
isr_ticker .usect "pmsmsmo",1 ; ISR checking variable
******************************************
* 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
Init_rtmon .if real_time ; Initialize real-time monitor
CALL MON_RT_CNFG ; For Real-Time
ldp #IMR>>7 ;
splk #01000000b,IMR ;
.else
ldp #IMR>>7 ;
splk #00000000b,IMR ;
.endif
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)
sar AR6,*+ ; save AR6 (used by DAC)
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
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -