pmsm_control.asm
来自「永磁同步电机驱动控制」· 汇编 代码 · 共 1,142 行 · 第 1/2 页
ASM
1,142 行
ldpk _isd
lacc #0
sub _isd
sacl _current_id_err
lt _current_id_err
mpy _current_kp
spm 3
pac
sfr
sfr
sacl _current_udref
current_pi_id_i:
mpy _current_ki
;b Buck_cc
spm 0
pac
and #07ffh
add _current_pi_error2
sacl _current_pi_error2
sub #2048
blz Buck_cc
sacl _current_pi_error2
spm 3
pac
sfr
sfr
sfr
sfr
sfr
add #1
add _current_id_sum
sacl _current_id_sum
b pi_onit2
Buck_cc:
spm 3
pac
sfr
sfr
sfr
sfr
sfr
add _current_id_sum
sacl _current_id_sum
pi_onit2:
lacc _current_id_sum
abs
sub #9240
blz yg2
lacc _current_id_sum
blz no_neg_id
splk #9240,_current_id_sum
b yg2
no_neg_id:
splk #-9240,_current_id_sum
yg2:
lacc _current_id_sum
;b Buck_dd
and #07h
add _current_pi_error3
sacl _current_pi_error3
sub #8
blz Buck_dd
sacl _current_pi_error3
lacc _current_id_sum
rpt #2
sfr
add #1
add _current_udref
sacl _current_udref
b current_pi_id_d
Buck_dd:
lacc _current_id_sum
rpt #2
sfr
add _current_udref
sacl _current_udref
current_pi_id_d:
lacc _current_id_err
sub _current_id_err_old
sacl _pi_temp
lt _pi_temp
mpy _current_kd
spm 3
pac
sfr
sfr
add _current_udref
sacl _current_udref
Buck_pi_limit_id:
lacc _current_udref
abs
sub #1155
BLZ Buck_pwm_over_id ;限幅值
lacc _current_uqref
blz Buck_u_limit_id
SPLK #1155,_current_udref
B Buck_pwm_over_id
Buck_u_limit_id:
SPLK #-1155,_current_udref
Buck_pwm_over_id:
lacc _current_id_err
sacl _current_id_err_old
.endif ;d轴电流调节结束
;================================================
.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
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变换结束
;===========================================================================
.if inv_park ;PARK反变换
;===========================================================================
; vsbeta_ref=vsq_ref*cosTeta_cm+vsd_ref*sinTeta_cm
; vsalfa_ref=-vsq_ref*sinTeta_cm+vsd_ref*cosTeta_cm
;===========================================================================
setc SXM
spm 0
lacc #0
ldpk _current_udref
lt _current_udref
mpy _sin_value
lta _current_uqref
mpy _cos_value
mpya _sin_value
sach _vsbeta_ref,1
lacc #0
lt _current_udref
mpys _cos_value
apac
sach _vsalfa_ref,1
.endif ;PARK反变换结束
;==========================================================================
.if sin_cos ;sin and cos cal
;==========================================================================
spm 0
setc sxm
ldpk _thet
lt _thet
mpy _thet_count
spm 0
pac
sacl _thet1
lacc _thet1
bgz onit
add #A360
sacl _thet1
onit:
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 svpwm_modulation ;svpwm modulation
;===========================================================================
setc SXM
ldpk _vsbeta_ref
lacc _vsbeta_ref
sacl _vref1
lt _vsalfa_ref
mpy #sqrt32
spm 0
pac
sach _vref4,4
lacc _vsbeta_ref
sfr
neg
sacl _vref5
add _vref4
sacl _vref2
lacc _vref5
sub _vref4
sacl _vref3
lacc _vref1
sacl _x
lacc _vref4
sub _vref5
sacl _y
lacc _vref4
add _vref5
neg
sacl _z
lacc _vref1
bgz ua
splk #0,_a
b uaon
ua:
splk #1,_a
uaon:
lacc _vref2
bgz ub
splk #0,_b
b ubon
ub:
splk #1,_b
ubon:
lacc _vref3
bgz uc
splk #0,_c
b ucon
uc:
splk #1,_c
ucon:
lacc _c,2
add _b,1
add _a
sacl _sector
sub #1
bz f_sector1
sub #1
bz f_sector2
sub #1
bz f_sector3
sub #1
bz f_sector4
sub #1
bz f_sector5
sub #1
bz f_sector6
b cmp_ok
f_sector1:
lacc _z
sacl _t1
lacc _y
sacl _t2
b t1t2out
f_sector2:
lacc _y
sacl _t1
lacc _x
neg
sacl _t2
b t1t2out
f_sector3:
lacc _z
neg
sacl _t1
lacc _x
sacl _t2
b t1t2out
f_sector4:
lacc _x
neg
sacl _t1
lacc _z
sacl _t2
b t1t2out
f_sector5:
lacc _x
sacl _t1
lacc _y
neg
sacl _t2
b t1t2out
f_sector6:
lacc _y
neg
sacl _t1
lacc _z
neg
sacl _t2
t1t2out:
b t1t2_ok
lt _t1
mpy #pwmprd
spm 1
pac
sach _t1,3
lt _t2
mpy #pwmprd
spm 1
pac
sach _t2 ,3
t1t2_ok:
lacc _t1
bgz t1_ok
splk #0,_t1
t1_ok:
lacc _t2
bgz t2_ok
splk #0,_t2
t2_ok:
lacc _t1
add _t2
sacl _tmp
sub #pwmprd
blez nosaturation
lacc #pwmprd,15
rpt #15
subc _tmp
sacl _tmp
lt _tmp
mpy _t1
spm 0
pac
sach _t1,1
mpy _t2
pac
sach _t2,1
nosaturation:
lacc #pwmprd
sub _t1
sub _t2
sfr
sacl _taon
add _t1
sacl _tbon
add _t2
sacl _tcon
lacc _sector
sub #1
bnz nosect1
lacc _tbon
sacl _cmp1
lacc _taon
sacl _cmp2
lacc _tcon
sacl _cmp3
b cmp_ok
nosect1:
sub #1
bnz nosect2
lacc _taon
sacl _cmp1
lacc _tcon
sacl _cmp2
lacc _tbon
sacl _cmp3
b cmp_ok
nosect2:
sub #1
bnz nosect3
lacc _taon
sacl _cmp1
lacc _tbon
sacl _cmp2
lacc _tcon
sacl _cmp3
b cmp_ok
nosect3:
sub #1
bnz nosect4
lacc _tcon
sacl _cmp1
lacc _tbon
sacl _cmp2
lacc _taon
sacl _cmp3
b cmp_ok
nosect4:
sub #1
bnz nosect5
lacc _tcon
sacl _cmp1
lacc _taon
sacl _cmp2
lacc _tbon
sacl _cmp3
b cmp_ok
nosect5:
lacc _tbon
sacl _cmp1
lacc _tcon
sacl _cmp2
lacc _taon
sacl _cmp3
cmp_ok:
lacc #pwmprd ;反向逻辑需要进行以下修正
sub _cmp1
sacl _cmp1
lacc #pwmprd
sub _cmp2
sacl _cmp2
lacc #pwmprd
sub _cmp3
sacl _cmp3
;================================
;这里要考虑实际输出的矢量与电机正
;方向的关系，来决定上述是否要减
;================================
lacc _n_ref
bnz back
splk #0,_speed_err
splk #0,_speed_err_old
splk #0,_current_given
splk #0,_speed_pi_error
splk #0,_speed_pi_error1
splk #0,_speed_sum
splk #0,_t2_sum
splk #0,_current_iq_err
splk #0,_current_iq_err_old
splk #0,_current_uqref
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,_thet
splk #0,_thet1
splk #0,_cmp1
splk #0,_cmp2
splk #0,_cmp3
.endif
back:
ret
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?