📄 pmsm3_1.asm
字号:
;==============================================================================
; File Name: PMSM3_1.ASM (QEP Version)
;
; System Name: PMSM3_1
;
; Description: Sensored Field Orientation Control for a Three Phase
; Permanent-Magnet Synchronous Motor using measured rotor angle.
;
; Originator/s: Digital control systems Group - Texas Instruments
;
; Target dependency: x240/1/2/3/07
; To Select the target device see x24x_app.h file.
;
; Last Update: 04-07-03
; ____________________________________________________________________________
; Date of Mod | DESCRIPTION
; ------------|---------------------------------------------------------------
; 04-07-03 | System created
; |
;==============================================================================
;******************************************************************************
; SYSTEM OPTIONS
;******************************************************************************
real_time .set 1 ; 1 for real time mode, otherwise set 0
;******************************************************************************
;-----------------------------------------------------------------------------
phase1_inc_build .set 1 ; SVGEN_DQ and FC_PWM_DRV tests
phase2_inc_build .set 0 ; Currents measurement test
phase3_inc_build .set 0 ; Two current PI regulator tests
phase4_inc_build .set 0 ; QEP test and angle calibration
phase5_inc_build .set 0 ; Speed closed loop using measured speed based QEP
;-----------------------------------------------------------------------------
; External references
;-----------------------------------------------------------------------------
.include "x24x_app.h"
.global MON_RT_CNFG
.ref SYS_INIT
.ref DAC_VIEW_DRV,DAC_VIEW_DRV_INIT ; function call
.ref DAC_IPTR0,DAC_IPTR1,DAC_IPTR2,DAC_IPTR3 ; Inputs
.ref RAMP_CNTL, RAMP_CNTL_INIT ; function call
.ref target_value ; Inputs
.ref rmp_dly_max, rmp_lo_limit ; Input Parameters
.ref rmp_hi_limit ; Input Parameter
.ref setpt_value, s_eq_t_flg ; Outputs
.ref FC_PWM_DRV, FC_PWM_DRV_INIT ;function call
.ref Mfunc_c1, Mfunc_c2, Mfunc_c3, Mfunc_p ;Inputs
.ref CLARKE, CLARKE_INIT ;function call
.ref clark_a, clark_b ;Inputs
.ref clark_d, clark_q ;Outputs
.ref ILEG2_DCBUS_DRV, ILEG2_DCBUS_DRV_INIT ;function call
.ref Ch_sel, Imeas_a_gain, Imeas_b_gain, Vdc_meas_gain ;Inputs
.ref Imeas_a_offset, Imeas_b_offset, Vdc_meas_offset ;Inputs
.ref Imeas_a,Imeas_b,Imeas_c ;Outputs
.ref Vdc_meas ;Outputs
.ref I_PARK, I_PARK_INIT ;function call
.ref ipark_D, ipark_Q, theta_ip ;Inputs
.ref ipark_d, ipark_q ;Outputs
.ref PARK, PARK_INIT ;function call
.ref park_d, park_q, theta_p ;Inputs
.ref park_D, park_Q ;Outputs
.ref RAMP_GEN, RAMP_GEN_INIT ;function call
.ref rmp_gain, rmp_offset, rmp_freq ;Inputs
.ref step_angle_max ;Input
.ref rmp_out ;Outputs
.ref SVGEN_DQ,SVGEN_DQ_INIT ;function call
.ref Ualfa,Ubeta ;Inputs
.ref Ta,Tb,Tc ;Outputs
.ref PID_REG3_ID,PID_REG3_ID_INIT ; function call
.ref id_fdb,id_ref ; Inputs
.ref ud_out ; Outputs
.ref PID_REG3_IQ,PID_REG3_IQ_INIT ; function call
.ref iq_fdb,iq_ref ; Inputs
.ref uq_out ; Outputs
.ref PID_REG3_SPD,PID_REG3_SPD_INIT ; function call
.ref spd_fdb,spd_ref ; Inputs
.ref t_out ; Outputs
.ref EN_DRIVE,DIS_DRIVE,DRIVE_INIT ; function call
.ref QEP_THETA_DRV,QEP_THETA_DRV_INIT ; function call
.ref polepairs,cal_angle,mech_scale ; Inputs
.ref theta_elec,theta_mech,dir_QEP ; Outputs
.ref QEP_INDEX_ISR_DRV ; function call
.ref index_sync_flg,QEP_cnt_idx ; Output
.ref SPEED_FRQ, SPEED_FRQ_INIT ;function call
.ref theta_elec_fr ;Inputs
.ref speed_frq, speed_frq_rpm ;Outputs
;
;-----------------------------------------------------------------------------
; Local Variable Declarations
;-----------------------------------------------------------------------------
.def GPR0 ;General purpose registers.
.def GPR1
.def GPR2
.def GPR3
.def GPR4
.def COMCON
.bss GPR0,1 ;General purpose registers.
.bss GPR1,1
.bss GPR2,1
.bss GPR3,1
.bss GPR4,1
.bss speed_ref,1
.bss speed_ref_rpm,1
.bss Vd_testing,1
.bss Vq_testing,1
.bss Id_ref,1
.bss Iq_ref,1
.bss lockrtr_flg,1
.bss enable_flg,1
.bss pdpint_flg,1
.bss sp_loop_cnt,1
.bss sp_loop_max,1
.bss isr_ticker,1
;==============================================================================
; V E C T O R T A B L E ( including RT monitor traps )
;==============================================================================
.include "c200mnrt.i" ; Include conditional assembly options.
.sect "vectors"
.def _c_int0
.def QEP_ISR ; int4-EV group C dispatcher/service
RESET B _c_int0 ; 00
INT1 B _c_int1 ; 02
INT2 B T1UF_ISR ; 04
INT3 B PHANTOM ; 06
INT4 B QEP_ISR ; 08
INT5 B PHANTOM ; 0A
INT6 B PHANTOM ; 0C
.include "rtvecs.h"
; Note : The above include line must be AFTER the user configurable
; vectors. Do not change the place where this line is included.
;==============================================================================
; M A I N C O D E - starts here
;==============================================================================
.text
_c_int0:
CALL SYS_INIT
CALL DAC_VIEW_DRV_INIT
CALL RAMP_GEN_INIT
CALL RAMP_CNTL_INIT
CALL SVGEN_DQ_INIT
CALL FC_PWM_DRV_INIT
CALL ILEG2_DCBUS_DRV_INIT
CALL CLARKE_INIT
CALL I_PARK_INIT
CALL PARK_INIT
CALL PID_REG3_ID_INIT
CALL PID_REG3_IQ_INIT
CALL PID_REG3_SPD_INIT
CALL DRIVE_INIT
CALL QEP_THETA_DRV_INIT
CALL SPEED_FRQ_INIT
;------------------------------------------------------------------------------------------
;System time-base init
;------------------------------------------------------------------------------------------
; Here time base is derived from T1 Underflow Int (i.e. Period)
; in FC_PWM_DRV_INIT module (see PWM_DRV.ASM).
;----------------------------------------------------------
; Initialise the Real time monitor
;----------------------------------------------------------
;---Real Time option---------------
.if (real_time)
CALL MON_RT_CNFG ;For Real-Time
.endif
;----------------------------------
.if (real_time) ; Wait for manually enable drive
POINT_B0
SPLK #0,enable_flg
WAIT_ENABLE LACC enable_flg
BCND WAIT_ENABLE,EQ
.endif
CALL EN_DRIVE ; Enable drive
;----------------------------------------------------------
; System Interrupt Init.
;----------------------------------------------------------
;Event Manager
POINT_EV
; SPLK #0000001000000000b,IMRA ;Enable T1 Underflow Int (i.e. Period)
SPLK #0000001000000001b,IMRA ;Enable T1 Underflow Int (i.e. Period)
; SPLK #0000000000000100b,IMRB ;Enable T2 Underflow Int (i.e. Period)
SPLK #0000000000000100b,IMRC ;Enable CAP3 int (i.e. QEP index pulse)
;||||!!!!||||!!!!
;5432109876543210
SPLK #0FFFFh,IFRA ; Clear all Group A interrupt flags
SPLK #0FFFFh,IFRB ; Clear all Group B interrupt flags
SPLK #0FFFFh,IFRC ; Clear all Group C interrupt flags
;C2xx Core
POINT_PG0
;---Real Time option --------------------------------------------------
.if (real_time)
SPLK #0000000001001010b,IMR ;En Int lvl 2,4,7 (T1 ISR, CAP3 ISR)
.endif
.if (real_time != 1)
SPLK #0000000000001010b,IMR ;En Int lvl 2,4 (T1 ISR, CAP3 ISR)
; SPLK #0000000000000100b,IMR ;En Int lvl 3 (T2 ISR)
;||||!!!!||||!!!!
;5432109876543210
.endif
SPLK #0FFFFh, IFR ;Clear any pending Ints
EINT ;Enable global Ints
;-----------------------------------------------------------------------
;Enables PWM signals on DMC1500
;-----------------------------------------------------------------------
.if (x243|x2407) ;target dependancy
POINT_PF2
LACC OCRA
AND #0BFFFh
SACL OCRA ;Select Secondary function IOPB6
LACC PBDATDIR
OR #04000h
SACL PBDATDIR ;Set IOPB6 as output
LACC PBDATDIR
AND #0FFBFh ;Set IOPB6 low, Enable PWM
; OR #00040h ;Set IOPB6 high, Disable PWM
SACL PBDATDIR
.endif
;---------------------------------------------------------
;SYSTEM PHASE INCREMENTAL BUILD OPTIONS - Initialization
;---------------------------------------------------------
POINT_B0
;---------------------------------------------------------
.if (phase1_inc_build)
SPLK #03000h, speed_ref ; Q15
SPLK #02000h, Vd_testing ; Q15
SPLK #00000h, Vq_testing ; Q15
SPLK #0DEADh, Id_ref ; (not used)
SPLK #0DEADh, Iq_ref ; (not used)
SPLK #0DEADh, lockrtr_flg ; (not used)
SPLK #0,pdpint_flg ; reset PDPINT_FLG
LDP #step_angle_max ; for Ramp generator
SPLK #655,step_angle_max ; correspond to fbase=200Hz, fs=20kHz
; SPLK #1311,step_angle_max ; correspond to fbase=400Hz, fs=20kHz
LDP #rmp_dly_max ; for Ramp control
SPLK #120,rmp_dly_max
SPLK #8000h,rmp_lo_limit
SPLK #7FFFh,rmp_hi_limit
LDP #DAC_IPTR0
SPLK #Ta,DAC_IPTR0
SPLK #Tb,DAC_IPTR1
SPLK #Tc,DAC_IPTR2
SPLK #rmp_out,DAC_IPTR3
.endif
;---------------------------------------------------------
;---------------------------------------------------------
.if (phase2_inc_build)
SPLK #03000h, speed_ref ; Q15
SPLK #02000h, Vd_testing ; Q15
SPLK #00000h, Vq_testing ; Q15
SPLK #0DEADh, Id_ref ; (not used)
SPLK #0DEADh, Iq_ref ; (not used)
SPLK #0DEADh, lockrtr_flg ; (not used)
SPLK #0,pdpint_flg ; reset PDPINT_FLG
LDP #step_angle_max ; for Ramp generator
SPLK #655,step_angle_max ; correspond to fbase=200Hz, fs=20kHz
; SPLK #1311,step_angle_max ; correspond to fbase=400Hz, fs=20kHz
LDP #cal_angle ; for QEP driver
SPLK #-2365,cal_angle ; used for 320V Pacific Sci. PMSM
LDP #rmp_dly_max ; for Ramp control
SPLK #120,rmp_dly_max
SPLK #8000h,rmp_lo_limit
SPLK #7FFFh,rmp_hi_limit
LDP #DAC_IPTR0
SPLK #clark_d,DAC_IPTR0
SPLK #clark_q,DAC_IPTR1
SPLK #clark_d,DAC_IPTR2
SPLK #clark_q,DAC_IPTR3
.endif
;---------------------------------------------------------
;---------------------------------------------------------
.if (phase3_inc_build)
SPLK #01000h, speed_ref ; Q15
SPLK #0DEADh, Vd_testing ; (not used)
SPLK #0DEADh, Vq_testing ; (not used)
SPLK #0DEADh, lockrtr_flg ; (not used)
SPLK #0,pdpint_flg ; reset PDPINT_FLG
SPLK #0000h, Id_ref ; Q15
SPLK #2500h, Iq_ref ; Q15
LDP #step_angle_max ; for Ramp generator
SPLK #655,step_angle_max ; correspond to fbase=200Hz, fs=20kHz
; SPLK #1311,step_angle_max ; correspond to fbase=400Hz, fs=20kHz
LDP #rmp_dly_max ; for Ramp control
SPLK #120,rmp_dly_max
SPLK #8000h,rmp_lo_limit
SPLK #7FFFh,rmp_hi_limit
LDP #DAC_IPTR0
SPLK #Imeas_a,DAC_IPTR0
SPLK #Tb,DAC_IPTR1
SPLK #Tc,DAC_IPTR2
SPLK #rmp_out,DAC_IPTR3
.endif
;---------------------------------------------------------
;---------------------------------------------------------
.if (phase4_inc_build)
SPLK #01000h, speed_ref ; Q15
SPLK #0DEADh, Vd_testing ; (not used)
SPLK #0DEADh, Vq_testing ; (not used)
SPLK #0,lockrtr_flg ; reset locked-rotor
SPLK #0,pdpint_flg ; reset PDPINT_FLG
SPLK #0000h, Id_ref ; Q15
SPLK #2500h, Iq_ref ; Q15
LDP #step_angle_max ; for Ramp generator
SPLK #655,step_angle_max ; correspond to fbase=200Hz, fs=20kHz
; SPLK #1311,step_angle_max ; correspond to fbase=400Hz, fs=20kHz
LDP #cal_angle ; for QEP driver
SPLK #-2365,cal_angle ; used for 320V Pacific Sci. PMSM
LDP #rmp_dly_max ; for Ramp control
SPLK #120,rmp_dly_max
SPLK #8000h,rmp_lo_limit
SPLK #7FFFh,rmp_hi_limit
LDP #DAC_IPTR0
SPLK #Ta,DAC_IPTR0
SPLK #Tb,DAC_IPTR1
SPLK #Tc,DAC_IPTR2
SPLK #Tc,DAC_IPTR3
.endif
;---------------------------------------------------------
;---------------------------------------------------------
.if (phase5_inc_build)
SPLK #01000h, speed_ref ; Q15
SPLK #0DEADh, Vd_testing ; (not used)
SPLK #0DEADh, Vq_testing ; (not used)
SPLK #0DEADh, lockrtr_flg ; (not used)
SPLK #0,pdpint_flg ; reset PDPINT_FLG
SPLK #0,sp_loop_cnt ; Q0 (speed counter)
SPLK #10,sp_loop_max ; Q0 (speed loop pre-scale)
SPLK #0000h, Id_ref ; Q15
SPLK #2500h, Iq_ref ; Q15
LDP #cal_angle ; for QEP driver
SPLK #-2365,cal_angle ; used for 320V Pacific Sci. PMSM
LDP #rmp_dly_max ; for Ramp control
SPLK #120,rmp_dly_max
SPLK #8000h,rmp_lo_limit
SPLK #7FFFh,rmp_hi_limit
LDP #DAC_IPTR0
SPLK #Imeas_a,DAC_IPTR0
SPLK #Ta,DAC_IPTR1
SPLK #Tb,DAC_IPTR2
SPLK #Tc,DAC_IPTR3
.endif
;---------------------------------------------------------
;======================================================
MAIN: ;Main system background loop
;======================================================
M_1
B MAIN
;======================================================
;===========================================================================
; Routine Name: T1UF_ISR Routine Type: ISR
;
; Description:
;
;
; Originator: Digital control systems Group - Texas Instruments
;
; History:
;
;
; Last Update: 13-Jan 00
;===========================================================================
T1UF_ISR:
;Context save regs
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 AR6,*+ ; save AR6 (used by DAC)
POPD *+ ; save TOS to free h/w stack
;More context save if needed
POINT_EV
SPLK #0FFFFh,IFRA ; Clear all Group A interrupt flags (T1 ISR)
;Checking bandwidth of ISR
CLRC XF
;=========================================================
;Start main section of ISR
;=========================================================
POINT_B0
; verifying the ISR
LACC isr_ticker
ADD #1
SACL isr_ticker
;---------------------------------------------------------
;SYSTEM PHASE INCREMENTAL BUILD OPTIONS - Main code
;---------------------------------------------------------
;---------------------------------------------------------
.if (phase1_inc_build)
;---------------------------------------------------------
LDP #target_value
BLDD #speed_ref,target_value
CALL RAMP_CNTL
LDP #rmp_freq
BLDD #setpt_value,rmp_freq
CALL RAMP_GEN
LDP #theta_ip
BLDD #rmp_out,theta_ip
BLDD #Vd_testing,ipark_D
BLDD #Vq_testing,ipark_Q
CALL I_PARK
LDP #Ualfa
BLDD #ipark_d,Ualfa
BLDD #ipark_q,Ubeta
CALL SVGEN_DQ
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -