pmsm3_2.asm

本程序来自TI公司网站原程序,其功能是通过传统的矢量控制算法来实现对永磁同步电机的控制,矢量控制采用双闭环结构

		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





**********************************************************
* Initialize Incremental Builds of Main Control Loop
**********************************************************
mloop_init
		.if I_build1		; Forward control path
		CALL	I_PARK_INIT		; Init inverse Parke routine
		CALL	SVGEN_DQ_INIT	; Init SV PWM routine
		CALL	FC_PWM_DRV_INIT	; Configure PWM
		ldp	#n_period		; Init T1/PWM period (norminal)
		splk	#T1PER_,n_period	;

		CALL	RAMP_CNTL_INIT	; Init ramping control
		ldp	#setpt_value
		SPLK	#200h,setpt_value	; Init set point
		SPLK	#0A00h,target_value; init target
		CALL	RAMP_GEN_INIT	; Init ramp gen to generate theta
		ldp	#rmp_offset
		SPLK	#3FFFh,rmp_offset	; Set offset to be 50%
		SPLK	#3FFFh,rmp_gain	; Set gain to be 50%
		CALL	DAC_VIEW_DRV_INIT	; Set up DAC to view vars
		ldp	#DAC_IPTR0
		SPLK	#rmp_out,DAC_IPTR0
		SPLK	#ipark_d,DAC_IPTR1
		SPLK	#ipark_q,DAC_IPTR2
		SPLK	#Ta,DAC_IPTR3
		.endif

		.if I_build2		; Current sensing and feedback path
		CALL	ILEG2DRV_INIT	; Init 2-shunt currrent sensing
		CALL	CLARKE_INIT		; Init Clarke routine
		CALL	PARK_INIT		; Init I Parke routine

		CALL	I_PARK_INIT		; Init inverse Parke routine
		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	;

		CALL	RAMP_CNTL_INIT	; Init ramping control
		ldp	#setpt_value
		SPLK	#200h,setpt_value	; Set point
		SPLK	#0A00h,target_value; Target point
		CALL	RAMP_GEN_INIT	; Init ramp gen to gen theta
		ldp	#rmp_offset
		SPLK	#3FFFh,rmp_offset	; Set offset to be 50%
		SPLK	#3FFFh,rmp_gain	; Set gain to be 50%
		CALL	DAC_VIEW_DRV_INIT	; Set up DAC to view vars
		ldp	#DAC_IPTR0
		SPLK	#Ia_out,DAC_IPTR0
		SPLK	#Ib_out,DAC_IPTR1
		SPLK	#clark_d,DAC_IPTR2
		SPLK	#park_D,DAC_IPTR3
		.endif

		.if I_build3		; D&Q current loops and regulators
		CALL	ILEG2DRV_INIT	; Init 2-shunt currrent sensing
		CALL	CLARKE_INIT		; Init Clarke routine
		CALL	PARK_INIT		; Init I Parke routine

		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	#0,id_ref		; Set D-axis current reference
		CALL	I_PARK_INIT		; Init inverse Parke routine
		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	;

		CALL	RAMP_CNTL_INIT	; Init ramping control
		ldp	#setpt_value
		SPLK	#200h,setpt_value	; Set point
		SPLK	#0A00h,target_value; Target point
		CALL	RAMP_GEN_INIT	; Init ramp gen to gen theta
		ldp	#rmp_offset
		SPLK	#3FFFh,rmp_offset	; Set offset to be 50%
		SPLK	#3FFFh,rmp_gain	; Set gain to be 50%
		CALL	DAC_VIEW_DRV_INIT	; Set up DAC to view vars
		ldp	#DAC_IPTR0		;
		SPLK	#iq_fdb,DAC_IPTR0
		SPLK	#iq_ref,DAC_IPTR1
		SPLK	#rmp_out,DAC_IPTR2
		SPLK	#id_fdb,DAC_IPTR3
		.endif

		.if I_build4		; Encoder i/f driver and calibration
		call	QEP_THETA_DRV_INIT; Initialize QEP driver
		ldp	#cal_angle		; Set encoder cal angle
		splk	#CAL_ANGLE_,cal_angle
		CALL	ILEG2DRV_INIT	; Init 2-shunt currrent sensing
		CALL	CLARKE_INIT		; Init Clarke routine
		CALL	PARK_INIT		; Init I Parke routine

		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	#0,id_ref		; Set D-axis current reference
		CALL	I_PARK_INIT		; Init inverse Parke routine
		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	#lockrt_flg		;
		splk	#0,lockrt_flg	; Reset lock rotor flag

		CALL	RAMP_CNTL_INIT	; Init ramping control
		ldp	#setpt_value
		SPLK	#200h,setpt_value	; Init set point
		SPLK	#800h,target_value; init target
		CALL	RAMP_GEN_INIT	; Init ramp gen to generate theta
		ldp	#rmp_offset
		SPLK	#3FFFh,rmp_offset	; Set offset to be 50%
		SPLK	#3FFFh,rmp_gain	; Set gain to be 50%
		CALL	DAC_VIEW_DRV_INIT	; Set up DAC to view vars
		ldp	#DAC_IPTR0
		SPLK	#rmp_out,DAC_IPTR0
		SPLK	#Ia_out,DAC_IPTR1
		SPLK	#theta_elec,DAC_IPTR2
		SPLK	#theta_mech,DAC_IPTR3
		.endif

		.if I_build5		; Current mode FOC and current regulators
		call	QEP_THETA_DRV_INIT; Initialize QEP driver
		ldp	#cal_angle		; Set encoder cal angle
		splk	#CAL_ANGLE_,cal_angle
		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	#lockcnt		;
		splk	#7FFFh,lockcnt	; initialize lock rotor counter
		splk	#1,lockrt_flg	; set lock rotor flag

		CALL	DAC_VIEW_DRV_INIT	; Set up DAC to view vars
		ldp	#DAC_IPTR0
		SPLK	#iq_ref,DAC_IPTR0
		SPLK	#iq_fdb,DAC_IPTR1
		SPLK	#theta_elec,DAC_IPTR2
		SPLK	#id_fdb,DAC_IPTR3
		.endif

		.if I_build6		; Speed calculator and speed loop
		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

		CALL	DAC_VIEW_DRV_INIT	; Set up DAC to view vars
		ldp	#DAC_IPTR0
		SPLK	#spd_ref,DAC_IPTR0
		SPLK	#speed_frq,DAC_IPTR1
		SPLK	#theta_mech,DAC_IPTR2
		SPLK	#theta_elec,DAC_IPTR3
		.endif

		.if I_build7		; SMO rotor angle estimator and SMO based FOC
		call	QEP_THETA_DRV_INIT; Initialize QEP driver
		ldp	#cal_angle		; Set encoder cal angle
		splk	#CAL_ANGLE_,cal_angle
		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	Smopos_init		; Init SMO rotor angle estimator

		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	#smo_flg		;
		splk	#0,smo_flg		; Use encoder feedback for FOC on startup
		splk	#7FFFh,lockcnt	; initialize lock rotor counter
		splk	#1,lockrt_flg	; set lock rotor flag

		CALL	DAC_VIEW_DRV_INIT	; Set up DAC to view vars
		ldp	#DAC_IPTR0
		SPLK	#theta_elec,DAC_IPTR0
		SPLK	#iq_ref,DAC_IPTR1
		SPLK	#thetae,DAC_IPTR2
		SPLK	#iq_fdb,DAC_IPTR3
		.endif

		.if I_build8		; Startup sequence
		call	QEP_THETA_DRV_INIT; Initialize QEP driver
		ldp	#cal_angle		; Set encoder cal angle
		splk	#CAL_ANGLE_,cal_angle
		CALL	ILEG2DRV_INIT	; Init 2-shunt current sensing
		CALL	CLARKE_INIT		; Init Clarke routine
		call	PARK_INIT		; Init parke
		call	Smopos_init		; Init SMO position observer

		call	pid_reg_id_init	; D-axis PI current regulator
		ldp	#id_ref		;
		splk	#0,id_ref		; Set Q-axis current reference
		call	pid_reg_iq_init	; Q-axis PI current regulator
		ldp	#iq_ref		;
		splk	#0400h,iq_ref	; Set Q-axis current reference
		CALL	I_PARK_INIT		; Init I Parke
		ldp	#ipark_D		; Set d and q voltage commands for initial ramp
		splk	#0400H,ipark_D
		splk	#0,ipark_Q		; lock rotor
		CALL	SVGEN_DQ_INIT	; Init SV PWM routine
		CALL	FC_PWM_DRV_INIT	; Configure PWM and PWM int
		ldp	#n_period		; Init T1/PWM period (norminal)
		splk	#T1PER_,n_period	;

		ldp	#lockcnt
		splk	#07FFFh,lockcnt	; Rotor lock counter
		splk	#07FFFh,rampcnt	; Initial ramp counter
		splk	#07FFFh,vconcnt	; Voltage control mode counter

		CALL	RAMP_CNTL_INIT	; Initialize ramp control
		ldp	#setpt_value
		SPLK	#200h,setpt_value	; Set point
		SPLK	#0A00h,target_value; Target point
		CALL	RAMP_GEN_INIT	; Init ramp gen
		ldp	#rmp_offset
		SPLK	#3FFFh,rmp_offset	; Set offset to be 50%
		SPLK	#3FFFh,rmp_gain	; Set gain to be 50%
		CALL	DAC_VIEW_DRV_INIT	; Set up DAC to view vars
		ldp	#DAC_IPTR0		;
		SPLK	#theta_elec,DAC_IPTR0
		SPLK	#Ualfa,DAC_IPTR1
		SPLK	#thetae,DAC_IPTR2
		SPLK	#theta_ip,DAC_IPTR3
		.endif

		; Init sections for more incremental builds
		ret
*** END Initialize Incremental Builds

**********************************************************
* Incrememtal Builds of Main Control Loop
**********************************************************
main_loop

		POINT_B0
		; verifying the ISR
		LACC 	isr_ticker	
		ADD		#1			
		SACL    isr_ticker 

		.if I_build1		; Forward control path and code framework
		ldp	#ipark_D		; Forward control path
		bldd	#setpt_value,ipark_D
		splk	#0,ipark_Q
		bldd	#rmp_out,theta_ip
		CALL	I_PARK
		ldp	#Ualfa
		bldd	#ipark_d,Ualfa
		bldd	#ipark_q,Ubeta
		CALL	SVGEN_DQ
		ldp	#Mfunc_c1
		bldd	#Ta,Mfunc_c1
		bldd	#Tb,Mfunc_c2
		bldd	#Tc,Mfunc_c3
		CALL	FC_PWM_DRV

		CALL	RAMP_CNTL		; Utilities
		ldp	#rmp_freq
		bldd	#setpt_value,rmp_freq
		CALL	RAMP_GEN
		CALL	DAC_VIEW_DRV
		.endif

		.if I_build2		; Currrent sensing and feedback path
		CALL	ILEG2DRV		; Feedback path
		ldp	#clark_a
		bldd	#Ia_out,clark_a
		bldd	#Ib_out,clark_b
		CALL	CLARKE
		ldp	#theta_p
		bldd	#rmp_out,theta_p
		bldd	#clark_d,park_d
		bldd	#clark_q,park_q
		call	PARK

		ldp	#ipark_Q		; Forward control path
		bldd	#setpt_value,ipark_Q
		splk	#0,ipark_D
		bldd	#rmp_out,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

		CALL	RAMP_CNTL		; Utilities
		ldp	#rmp_freq
		bldd	#setpt_value,rmp_freq