pmsm_control.asm
来自「永磁同步电机驱动控制」· 汇编 代码 · 共 1,262 行 · 第 1/3 页
ASM
1,262 行
.include "sintable.h"
;-----------------------------------------------------------
T4CNT .SET 7405H
pwmprd .set 1000 ;pwm 周期
sqrt3inv .set 093dh ;1/sqrt(3) Q12
sqrt32 .set 0ddbh ;sqrt(3)/2 Q12
A360 .set 2048*4 ;2000h
A270 .set 2048*3 ;1800h
A180 .set 2048*2 ;1000h
A90 .set 2048*1 ; 800h
;===========================================================
_temp .usect ".blk1", 1
_temp_h .usect ".blk1", 1
_temp_l .usect ".blk1", 1
_t23 .usect ".blk1", 1
_t22 .usect ".blk1", 1
_init_phase_flag .usect ".blk1", 1
.global _temp_h,_temp_l,_temp,_t23,_init_phase_flag
;===========================================================
_n_ref .usect ".blk1", 1
_n_feed .usect ".blk1", 1
_speed_err .usect ".blk1", 1
_speed_err_old .usect ".blk1", 1
_speed_kp .usect ".blk1", 1
_current_given .usect ".blk1", 1
_speed_ki .usect ".blk1", 1
_speed_kd .usect ".blk1", 1
_speed_pi_error .usect ".blk1", 1
_speed_pi_error1 .usect ".blk1", 1
_speed_sum .usect ".blk1", 1
_pi_temp .usect ".blk1", 1
_t2_add .usect ".blk1", 1
_t2_sum .usect ".blk1", 1
_speed_count .usect ".blk1", 1
_speed_number .usect ".blk1", 1
_SPEED_TEMP .usect ".blk1", 1
.global _n_ref,_n_feed,_speed_err,_speed_err_old
.global _speed_kp,_current_given,_speed_ki,_speed_kd
.global _speed_pi_error,_speed_pi_error1,_speed_sum,_pi_temp
.global _t2_sum,_speed_count,_SPEED_TEMP
;----------------------------------------------------------
_current_iq_err .usect ".blk1", 1
_current_iq_err_old .usect ".blk1", 1
_current_kp .usect ".blk1", 1
_current_uqref .usect ".blk1", 1
_current_ki .usect ".blk1", 1
_current_kd .usect ".blk1", 1
_current_pi_error .usect ".blk1", 1
_current_pi_error1 .usect ".blk1", 1
_current_iq_sum .usect ".blk1", 1
.global _current_iq_err,_current_iq_err_old,_current_kp,_current_uqref
.global _current_ki,_current_kd,_current_pi_error,_current_pi_error1
.global _current_iq_sum
;-----------------------------------------------------------
_current_id_err .usect ".blk1", 1
_current_id_err_old .usect ".blk1", 1
_current_udref .usect ".blk1", 1
_current_pi_error2 .usect ".blk1", 1
_current_pi_error3 .usect ".blk1", 1
_current_id_sum .usect ".blk1", 1
.global _current_id_err,_current_id_err_old,_current_udref,_current_pi_error2
.global _current_pi_error3,_current_id_sum
;-----------------------------------------------------------
_ia .usect ".blk1", 1
_ib .usect ".blk1", 1
_sampling_k .usect ".blk1", 1
_is_alfa .usect ".blk1", 1
_is_beta .usect ".blk1", 1
_isd .usect ".blk1", 1
_isq .usect ".blk1", 1
_is_temp .usect ".blk1", 1
_vsbeta_ref .usect ".blk1", 1
_vsalfa_ref .usect ".blk1", 1
.global _ia,_ib,_sampling_k,_is_alfa
.global _is_beta,_isd,_isq,_is_temp
.global _vsbeta_ref,_vsalfa_ref
;-----------------------------------------------------------
_thet .usect ".blk1", 1
_thet1 .usect ".blk1", 1
_thet_count .usect ".blk1", 1
_s_sin .usect ".blk1", 1
_s_cos .usect ".blk1", 1
_ang_cos .usect ".blk1", 1
_ang_sin .usect ".blk1", 1
_sin_temp .usect ".blk1", 1
_sin_value .usect ".blk1", 1
_cos_value .usect ".blk1", 1
.global _thet,_thet1,_s_sin,_s_cos
.global _ang_cos,_ang_sin,_sin_temp,_sin_value
.global _cos_value
;-----------------------------------------------------------
_vref1 .usect ".blk1", 1
_vref2 .usect ".blk1", 1
_vref3 .usect ".blk1", 1
_vref4 .usect ".blk1", 1
_vref5 .usect ".blk1", 1
_x .usect ".blk1", 1
_y .usect ".blk1", 1
_z .usect ".blk1", 1
_a .usect ".blk1", 1
_b .usect ".blk1", 1
_c .usect ".blk1", 1
_sector .usect ".blk1", 1
_t1 .usect ".blk1", 1
_t2 .usect ".blk1", 1
_taon .usect ".blk1", 1
_tbon .usect ".blk1", 1
_tcon .usect ".blk1", 1
_cmp1 .usect ".blk1", 1
_cmp2 .usect ".blk1", 1
_cmp3 .usect ".blk1", 1
_tmp .usect ".blk1", 1
.global _vref1,_vref2,_vref3,_vref4
.global _vref5,_x,_y,_z
.global _a,_b,_c,_sector
.global _t1,_t2,_taon,_tbon
.global _tcon,_cmp1,_cmp2,_cmp3
.global _tmp
;----------------------------------------------------------
; .def _speed_pi_loop,_current_pi_loop
; .global _speed_pi_loop,_current_pi_loop
; .def _current_clarke,_park_inv
; .global _current_clarke,_park_inv
; .def _Sincos
; .global _Sincos
; .def _svpwm_modulation
; .global _svpwm_modulation
; .def _arithmetic_foc
; .global _arithmetic_foc
.def _variable_initial
.global _variable_initial
.def _pmsm_control
.global _pmsm_control
cmpr_value .usect ".blk1", 1
temp1 .usect ".blk1", 1
ar4_temp .usect ".blk1", 1
ar6_temp .usect ".blk1", 1
;-----------------------------------------------------------------
;soft switches
speed_regulator .set 1
id_requlator .set 1
iq_requlator .set 1
current_clarke .set 1
current_park .set 1
inv_park .set 1
sin_cos .set 1
svpwm_modulation .set 1
;=================================================================
;变量初始化
;================================================
_variable_initial:
ldpk _n_ref
splk #0,_init_phase_flag
splk #0,_t23
splk #0,_t22
splk #0,_n_ref
splk #0,_n_feed
splk #0,_speed_err
splk #0,_speed_err_old
splk #100,_speed_kp
splk #0,_current_given
splk #40,_speed_ki
splk #10,_speed_kd
splk #0,_speed_pi_error
splk #0,_speed_pi_error1
splk #0,_speed_sum
splk #0,_pi_temp
splk #0,_t2_add
splk #0,_t2_sum
splk #15000,_speed_count ;Q8 1500=6000,58.59375
splk #20,_speed_number
splk #0,_current_iq_err
splk #0,_current_iq_err_old
splk #10,_current_kp
splk #0,_current_uqref
splk #5,_current_ki
splk #5,_current_kd
splk #0,_current_pi_error
splk #0,_current_pi_error1
splk #0,_current_iq_sum
splk #0,_current_id_err
splk #0,_current_id_err_old
splk #0,_current_udref
splk #0,_current_pi_error2
splk #0,_current_pi_error3
splk #0,_current_id_sum
splk #0,_ia
splk #0,_ib
splk #500,_sampling_k
splk #0,_is_alfa
splk #0,_is_beta
splk #0,_isd
splk #0,_isq
splk #0,_is_temp
splk #0,_vsbeta_ref
splk #0,_vsalfa_ref
splk #0,_thet
splk #0,_thet1
splk #4,_thet_count ;2对极 1024线脉冲
splk #0,_s_sin
splk #0,_s_cos
splk #0,_ang_cos
splk #0,_ang_sin
splk #0,_sin_temp
splk #0,_sin_value
splk #0,_cos_value
splk #0,_vref1
splk #0,_vref2
splk #0,_vref3
splk #0,_vref4
splk #0,_vref5
splk #0,_x
splk #0,_y
splk #0,_z
splk #0,_a
splk #0,_b
splk #0,_c
splk #0,_sector
splk #0,_t1
splk #0,_t2
splk #0,_taon
splk #0,_tbon
splk #0,_tcon
splk #0,_cmp1
splk #0,_cmp2
splk #0,_cmp3
splk #0,_tmp
lar ar4,#0a000h
lar ar6,#0a800h
splk #1,cmpr_value
splk #0a000h,ar4_temp
splk #0a800h,ar6_temp
ret
;=====================================================
;FOC算法
;=====================================================
_pmsm_control:
b start_run
ldpk _init_phase_flag
lacl _init_phase_flag
bcnd start_run,neq
lacc #8000h ;-90
sacl _thet1
lacc #1000h
sacl _current_given
lacc #0
sacl _thet
sacl _n_feed
sacl _t23
ldp #T4CNT>>7
splk #3,T4CNT
ldpk _init_phase_flag
b init
start_run:
;==========================================================================
; .if sin_cos ;sin and cos cal
;==========================================================================
setc sxm
ldpk _thet
lt _thet
mpy _thet_count
spm 0
pac
sacl _thet1
ppt_aa:
lacc _thet1
abs
sub #A360
blz onit
lacc _thet1
blz neg_limit11
sub #A360
sacl _thet1
b ppt_aa
neg_limit11:
lacc _thet1
add #A360
sacl _thet1
b ppt_aa
onit:
splk #0,_thet1
lacc _thet1
bgz next_ca
add #A360
sacl _thet1
next_ca:
lacc _thet1
sub #A270
blz cc2
splk #-1,_s_sin
splk #1,_s_cos
sacl _ang_cos
sub #A90
neg
sacl _ang_sin
b cc5
cc2:
add #A270
sub #A180
blz cc3
splk #-1,_s_sin
splk #-1,_s_cos
sacl _ang_sin
sub #A90
neg
sacl _ang_cos
b cc5
cc3:
add #A180
sub #A90
blz cc4
splk #1,_s_sin
splk #-1,_s_cos
sacl _ang_cos
sub #A90
neg
sacl _ang_sin
b cc5
cc4:
add #A90
splk #1,_s_sin
splk #1,_s_cos
sacl _ang_sin
sub #A90
neg
sacl _ang_cos
cc5:
lacc _ang_sin
add #Stable
tblr _sin_temp
lt _s_sin
mpy _sin_temp
spl _sin_value
lacc _ang_cos
add #Stable
tblr _sin_temp
lt _s_cos
mpy _sin_temp
spl _cos_value
; .endif ;;sin and cos cal 结束
;===========================================================================
; .if current_clarke ;CLARKE变换
;========================================================================
; clarke transformation
; i_alfa=ia;
; i_beta=(1/sqrt(3))*(2*ib+ia);
;========================================================================
; setc SXM
ldpk _ia
LACC _ia ;foc_isalfa=foc_ia
sacl _is_alfa ;foc_isalfa,4.12f
LACC _ib,1 ;foc_isbeta=(2*foc_ib+foc_ia)/sqrt(3)
add _ia
sacl _is_temp
lt _is_temp
mpy #sqrt3inv ;foc_sqrt3inv=(1/sqrt(3))=093dh,4.12f
spm 0
pac
sach _is_beta,4 ;foc_isbeta,4.12f
; .endif ;CLARKE变换结束
;========================================================================
; .if current_park ;PARK变换
;========================================================================
; park transformation
; isd=foc_isalfa*cosTeta_cm+foc_isbeta*sinTeta_cm
; isq=-foc_isalfa*sinTeta_cm+foc_isbeta*cosTeta_cm
;========================================================================
spm 0
lacc #0
lt _is_beta
mpy _sin_value
lta _is_alfa
mpy _cos_value
mpya _sin_value
sach _isd,1
lacc #0
lt _is_beta
mpys _cos_value
apac
sach _isq,1
; .endif ;PARK变换结束
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?