📄 主程序1.asm.txt
字号:
;=====================================================================
; System Name : ACI3_3
; File Name : ACI3_31.ASM
; Description: ACI 3-phase Field Oriented control with speed (using QEP)
; and current closed loop
;=====================================================================
******************************************
* Select PWM/T1 Period
******************************************
PWM_PERIOD .set 100 ; PWM period in uS (10KHz)
T1PER_ .set PWM_PERIOD*10 ; *1000nS/(2*50nS)
;*********************************************************************;---------------------------------------------------------------------
; External references
;---------------------------------------------------------------------
.include "x24x_app.h"
.ref SYS_INIT
.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 I_PARK, I_PARK_INIT ;function call
.ref ipark_D, ipark_Q, theta_ip ;Inputs
.ref ipark_d, ipark_q ;Outputs
.ref SVGEN_DQ,SVGEN_DQ_INIT ;function call
.ref Ualfa,Ubeta ;Inputs
.ref Ta,Tb,Tc ;Outputs
.ref FC_PWM_DRV,FC_PWM_DRV_INIT ;function calls
.ref Mfunc_c1,Mfunc_c2,Mfunc_c3,Mfunc_p ;Inputs
.ref n_period ;Input
.ref ILEG2DRV, ILEG2DRV_INIT ;function call
.ref Ia_gain,Ib_gain,Ia_offset,Ib_offset ;Inputs
.ref Ia_out, Ib_out ;Outputs
.ref CLARKE, CLARKE_INIT ;function call
.ref clark_a, clark_b ;Inputs
.ref clark_d, clark_q ;Outputs
.ref PARK, PARK_INIT ;function call
.ref park_d, park_q, theta_p ;Inputs
.ref park_D, park_Q ;Outputs
.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 shaft_angle, direction ;Inputs
.ref speed_frq, speed_frq_rpm ;Outputs
.ref pid_reg_id,pid_reg_id_init ;function call
.ref id_fdb,id_ref,Kp_d,Ki_d,Kc_d ;Inputs
.ref ud_int ;Input
.ref ud_out ;Outputs
.ref pid_reg_iq,pid_reg_iq_init ;function call
.ref iq_fdb,iq_ref,Kp_q,Ki_q,Kc_q ;Inputs
.ref uq_int ;Input
.ref uq_out ;Outputs
.ref CURRENT_MODEL,CURRENT_MODEL_INIT ;function call
.ref i_cur_mod_D,i_cur_mod_Q ;Inputs
.ref spd_cur_mod ;Input
.ref theta_cur_mod ;Outputs
.ref pid_reg_spd,pid_reg_spd_init ;function call
.ref spd_fdb,spd_ref ;Inputs
.ref spd_out ;Outputs
;---------------------------------------------------------------------
; Variable Declarations
;---------------------------------------------------------------------
.def GPR0 ;General purpose registers.
.bss GPR0,1 ;General purpose register
.bss my_iq_ref,1
.bss my_id_ref,1
.bss speed_reference,1
;=====================================================================
; V E C T O R T A B L E
;=====================================================================
.sect "vectors"
.def _c_int0
.def _c_int4 ;int4-EV group C dispatcher/service
RESET B _c_int0 ; 00
INT1 B PHANTOM ; 02
INT2 B T1_PERIOD_ISR ; 04
INT3 B PHANTOM ; 06
INT4 B _c_int4 ; 08
INT5 B PHANTOM ; 0A
INT6 B PHANTOM ; 0C
;=====================================================================
; M A I N C O D E - starts here
;=====================================================================
.text
_c_int0:
CALL SYS_INIT
CALL FC_PWM_DRV_INIT ; Here is defined the Timer
;frequency (10khz), it is thus mandatory
;to include this initialisation since
；the beginning
CALL I_PARK_INIT
CALL SVGEN_DQ_INIT
CALL ILEG2DRV_INIT
CALL CLARKE_INIT
CALL PARK_INIT
CALL QEP_THETA_DRV_INIT
CALL SPEED_FRQ_INIT
CALL pid_reg_id_init
CALL pid_reg_iq_init
CALL CURRENT_MODEL_INIT
CALL pid_reg_spd_init
;-----------------------------------
; Variables initialization
;----------------------------------
LDP #n_period
SPLK #T1PER_,n_period ;initialize the PWM period to 10kHz
LDP #rmp_freq
SPLK #2A00h,rmp_freq ;50Hz frequency for RAMPGEN
LDP #my_iq_ref
SPLK #0000h,my_iq_ref
LDP #my_id_ref
SPLK #0000h,my_id_ref
LDP #speed_reference
SPLK #0000h,speed_reference
POINT_B0
SPLK #0500h,my_id_ref
;----------------------------------------------------------
; System Interrupt Init.
;----------------------------------------------------------
;Event Manager
POINT_EV
SPLK #0000001000000000b,IMRA ;Enable T1 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
SPLK #0000000000001010b,IMR ;En Int lvl 4 (CAP3/QEP ISR)
;||||!!!!||||!!!!
;5432109876543210
SPLK #0FFFFh, IFR ;Clear any pending Ints
EINT ;Enable global Ints
POINT_B0
;---------------------------------------------------------------------
;======================================================
MAIN: ;Main system background loop
;======================================================
M_1 NOP
NOP
NOP
CLRC XF
B MAIN
;======================================================
;=====================================================================
; Routine Name: T1_PERIOD_ISR Routine Type: ISR
;=====================================================================
T1_PERIOD_ISR:
;Context save regs
MAR *,AR1 ;AR1 is stack pointer
MAR *+ ;skip one position
SST #1, *+ ;save ST1
SST #0, *+ ;save ST0
SACH *+ ;save acc high
SACL * ;save acc low
;NOTE: should use "read-modify-write" to clear Int flags & not SPLK!
POINT_EV
SPLK #0FFFFh,IFRA ; Clear all Group A interrupt flags
；(T1 ISR)
SETC XF
SETC SXM ; set sign extension mode
CLRC OVM ; clear overflow mode
;=========================================================
;Start main section of ISR
;=========================================================
; Current leg measurement, Ileg2drv module
CALL ILEG2DRV
; Clarke module
LDP #clark_a
BLDD #Ia_out,clark_a
BLDD #Ib_out,clark_b
CALL CLARKE
; QEP Module
CALL QEP_THETA_DRV
; SPEED REGULATION
LDP #spd_ref
bldd #speed_reference,spd_ref
bldd #spd_cur_mod,spd_fdb
CALL pid_reg_spd
; SPEED_FR Module
LDP #shaft_angle
BLDD #theta_mech,shaft_angle
BLDD #dir_QEP,direction
CALL SPEED_FRQ
; PARK module
LDP #park_d
BLDD #clark_d,park_d
BLDD #clark_q,park_q
BLDD #theta_cur_mod,theta_p
CALL PARK
; CURRENT MODEL
LDP #spd_cur_mod
BLDD #speed_frq,spd_cur_mod
BLDD #park_D,i_cur_mod_D
BLDD #park_Q,i_cur_mod_Q
CALL CURRENT_MODEL
; D-axis current regulator
LDP #id_ref
;SPLK #1EB8h,id_ref ;1EB8h gives a current of 2.5A peak
BLDD #my_id_ref,id_ref
BLDD #park_D,id_fdb
CALL pid_reg_id
; Q-axis current regulator
LDP #iq_ref
BLDD #spd_out,iq_ref
BLDD #park_Q,iq_fdb
CALL pid_reg_iq
; Inverse-Park module
LDP #ipark_D
BLDD #ud_out,ipark_D
BLDD #uq_out,ipark_Q
BLDD #theta_cur_mod,theta_ip
CALL I_PARK
; Space-Vector DQ module
LDP #Ualfa
BLDD #ipark_d,Ualfa
BLDD #ipark_q,Ubeta
CALL SVGEN_DQ
; PWM driver
LDP #Mfunc_c1
BLDD #Ta,Mfunc_c1
BLDD #Tb,Mfunc_c2
BLDD #Tc,Mfunc_c3
CALL FC_PWM_DRV
;=========================================================
;End main section of ISR
;=========================================================
;Context restore regs
END_ISR:
POINT_PG0
MAR *, AR1 ;make stack pointer active
LACL *- ;Restore Acc low
ADDH *- ;Restore Acc high
LST #0, *- ;load ST0
LST #1, *- ;load ST1
EINT
RET
*******************************************************
* 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
_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
;=========================================================
;End main section of ISR INT4 -EV GROUP C INTS
;=========================================================
;Context restore regs
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
;=====================================================================
; I S R - PHANTOM
; Description: Dummy ISR, used to trap spurious interrupts.
;=====================================================================
PHANTOM B PHANTOM
;=====================================================================
*** END Whole Code
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -