pmsm_control.asm
来自「永磁同步电机驱动控制」· 汇编 代码 · 共 1,142 行 · 第 1/2 页
ASM
1,142 行
.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
;-----------------------------------------------------------------
;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
ret
;=====================================================
;FOC算法
;=====================================================
_pmsm_control:
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 speed_regulator ;速度环
;====================================================
ldpk _t23
setc SXM
lacc _t23
sub _t22
;neg
sacl _t2_add
lacc _t23
sacl _t22
lacc _speed_number
sub #1
sacl _speed_number
bnz next_speed_loop
lt _t2_sum
mpy _speed_count
spm 3
pac
sfr
sfr
sacl _n_feed
splk #20,_speed_number
splk #0,_t2_sum
next_speed_loop:
lacc _n_ref
sub _n_feed
sacl _speed_err
lt _speed_err
mpy _speed_kp
spm 3
pac
sfr
sfr
sacl _current_given
speed_pi_i:
mpy _speed_ki
;b pip1
spm 0
pac
and #07ffh
add _speed_pi_error
sacl _speed_pi_error
sub #2048
blz pip1
sacl _speed_pi_error
spm 3
pac
sfr
sfr
sfr
sfr
sfr
add #1
add _speed_sum
sacl _speed_sum
b pip2
pip1:
spm 3
pac
sfr
sfr
sfr
sfr
sfr
add _speed_sum
sacl _speed_sum
pip2:
lacc _speed_sum
abs
sub #4000
blz yg2_speed
lacc _speed_sum
blz neg_limit_speed
lacc #4000
sacl _speed_sum
b yg2_speed
neg_limit_speed:
splk #-4000,_speed_sum
yg2_speed:
lacc _speed_sum
;b pip3
and #07h
add _speed_pi_error1
sacl _speed_pi_error1
sub #8
blz pip3
sacl _speed_pi_error1
lacc _speed_sum
rpt #2
sfr
add #1
add _current_given
sacl _current_given
b speed_pi_d
pip3:
lacc _speed_sum
rpt #2
sfr
add _current_given
sacl _current_given
speed_pi_d:
lacc _speed_err
sub _speed_err_old
sacl _pi_temp
lt _pi_temp
mpy _speed_kd
spm 3
pac
sfr
sfr
add _current_given
sacl _current_given
speed_pi_limit:
lacc _current_given
abs
sub #500
BLZ speed_over
lacc _current_given
blz neg_u_limit
lacc #500
sacl _current_given
B speed_over
neg_u_limit:
SPLK #-500,_current_given
speed_over:
lacc _speed_err
sacl _speed_err_old
lacc _t2_sum
add _t2_add
sacl _t2_sum
lacc _thet
add _t2_add
sacl _thet
.endif ;speed loop over
;================================================================
init:
;================================================================
.if iq_requlator ;iq电流调节
;================================================================
setc SXM
current_pi_p:
ldpk _current_given
lacc _current_given
sub _isq
sacl _current_iq_err
lt _current_iq_err
mpy _current_kp
spm 3
pac
sfr
sfr
sacl _current_uqref
current_pi_i:
mpy _current_ki
;b Buck_aa
spm 0
pac
and #07ffh
add _current_pi_error
sacl _current_pi_error
sub #2048
blz Buck_aa
sacl _current_pi_error
spm 3
pac
sfr
sfr
sfr
sfr
sfr
add #1
add _current_iq_sum
sacl _current_iq_sum
b pi_onit1
Buck_aa:
spm 3
pac
sfr
sfr
sfr
sfr
sfr
add _current_iq_sum
sacl _current_iq_sum
pi_onit1:
lacc _current_iq_sum
abs
sub #9240
blz yg1
lacc _current_iq_sum
blz no_neg
splk #9240,_current_iq_sum
b yg1
no_neg:
splk #-9240,_current_iq_sum
yg1:
lacc _current_iq_sum
;b Buck_bb
and #07h
add _current_pi_error1
sacl _current_pi_error1
sub #8
blz Buck_bb
sacl _current_pi_error1
lacc _current_iq_sum
rpt #2
sfr
add #1
add _current_uqref
sacl _current_uqref
b current_pi_d
Buck_bb:
lacc _current_iq_sum
rpt #2
sfr
add _current_uqref
sacl _current_uqref
current_pi_d:
lacc _current_iq_err
sub _current_iq_err_old
sacl _pi_temp
lt _pi_temp
mpy _current_kd
spm 3
pac
sfr
sfr
add _current_uqref
sacl _current_uqref
Buck_pi_limit:
lacc _current_uqref
abs
sub #1155
BLZ Buck_pwm_over ;限幅值
lacc _current_uqref
blz Buck_u_limit
SPLK #1155,_current_uqref
B Buck_pwm_over
Buck_u_limit:
SPLK #-1155,_current_uqref
Buck_pwm_over:
lacc _current_iq_err
sacl _current_iq_err_old
.endif ;q轴电流调节结束
;---------------------------------------------------------------------
;=====================================================
.if id_requlator ;d轴电流调节
;=====================================================
current_pi_id_p:
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?