pmsm3_2.asm

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

		CALL	RAMP_GEN
		CALL	DAC_VIEW_DRV
		.endif

		.if I_build3		; D&Q current loops and regulators
		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	#iq_ref		; Forward control path
            bldd	#setpt_value,iq_ref
            lacc	iq_ref,1
            sacl	iq_ref		; *2
		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	#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
		CALL	RAMP_GEN
		CALL	DAC_VIEW_DRV
		.endif

		.if I_build4		; Encoder i/f driver and calibration
Lockrt_test	ldp	#lockrt_flg
		lacc	lockrt_flg
		bcnd	Fwd_path,EQ
		ldp	#rmp_out
		splk	#0,rmp_out

Fwd_path	call	QEP_THETA_DRV	; Feedback paths
		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	#rmp_out,theta_p
		call	PARK

		ldp	#iq_ref		; Forward control path
		bldd	#park_Q,iq_fdb
		call	pid_reg_iq
		ldp	#id_fdb
            bldd	#setpt_value,id_ref
            lacc	id_ref,1
            sacl	id_ref		; *2
		bldd	#park_D,id_fdb
		call	pid_reg_id
		ldp	#ipark_Q
		bldd	#uq_out,ipark_Q
		bldd	#ud_out,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
		CALL	RAMP_GEN
		CALL	DAC_VIEW_DRV
		.endif

		.if I_build5		; Current mode FOC and current regulators
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
		ldp	#T2CNT>>7
		splk	#CAL_ANGLE_,T2CNT
		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	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

Forwd_cntl	ldp	#iq_fdb		; Forward control path
		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

		CALL	DAC_VIEW_DRV	; Utility
		.endif

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

		CALL	DAC_VIEW_DRV	; Utility
		.endif

		.if I_build7		; SMO rotor angle estimator and SMO based FOC
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

SMO_test	ldp	#smo_flg
            lacc	smo_flg
            bcnd	Fdb_path,EQ
            bldd	#thetae,temp0

Fdb_path	call	QEP_THETA_DRV	; Feedback paths
		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	#vsalfa
		bldd	#Ualfa,vsalfa
		bldd	#Ubeta,vsbeta
		bldd	#clark_d,isalfa
		bldd	#clark_q,isbeta
		splk	#spd_ref_,speedref
		call	Smopos

 		ldp	#iq_fdb		; Forward control path
		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

		CALL	DAC_VIEW_DRV	; Utility
		.endif

		.if I_build8		; Startup sequence
		call	QEP_THETA_DRV	; Feedback paths
		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	#thetae,theta_p
		call	PARK
		ldp	#vsalfa		; Pass arguments to Smopos
		bldd	#Ualfa,vsalfa
		bldd	#Ubeta,vsbeta
		bldd	#clark_d,isalfa
		bldd	#clark_q,isbeta
		splk	#spd_ref_,speedref
		call	Smopos

		; Speed controller
Lockrot	ldp	#lockcnt		;
		lacc	lockcnt
		bcnd	Ramping,LEQ
		sub	#1
		sacl	lockcnt
		ldp	#theta_ip
		splk	#0,theta_ip
		b	I_park

Ramping	ldp	#rampcnt
		lacc	rampcnt
		bcnd	Vonctrol,LEQ
		sub	#1
		sacl	rampcnt
		ldp	#ipark_D
		bldd	#setpt_value,ipark_D
		bldd	#rmp_out,theta_ip
		b	I_park		

Vonctrol	ldp	#vconcnt
		lacc	vconcnt
		bcnd	End_vcont,LEQ
		sub	#1
		sacl	vconcnt
		ldp	#ipark_Q		;
		splk	#0900h,ipark_Q	;
		splk	#0,ipark_D
		bldd	#thetae,theta_ip	;
		ldp	#uq_int		; Keep pid outputs updated
		bldd	#ipark_Q,uq_int
		bldd	#ipark_D,ud_int
		b	I_park

End_vcont	ldp	#id_fdb
		bldd	#park_D,id_fdb
		call	pid_reg_id
 		ldp	#iq_fdb
		bldd	#park_Q,iq_fdb
		call	pid_reg_iq
		ldp	#ipark_D
		bldd	#ud_out,ipark_D
		bldd	#uq_out,ipark_Q
		bldd	#thetae,theta_ip
I_park	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
PWM_drv	CALL	FC_PWM_DRV

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

		ret
*** END Incrememtal Builds

*** END Whole Code