📄 arithmetic.asm
字号:
neg_u_limit:
SPLK #-500,_current_given
speed_over:
lacc _speed_err
sacl _speed_err_old
;================================================
; SBRK #2
; LAR AR0,*-
; PSHD *
lacc _t2_sum
add _t2_add
sacl _t2_sum
lacc _thet
add _t2_add
sacl _thet
ret
;================================================
;iq,id两电流环
;================================================
_current_pi_loop:
;================================================
; POPD *+
; SAR AR0,*+
; SAR AR1,*
; LARK AR0,#1
; LAR AR0,*0+,AR0
;================================================
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
;============================================
;d轴电流环
;============================================
current_pi_id_p:
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
;================================================
;恢复C语言现场
;================================================
; SBRK #2
; LAR AR0,*-
; PSHD *
ret
;=================================================
;电流电压量坐标变换,包括CLARKE和PARK变换以及反变换
;=================================================
_current_clarke:
;=================================================
;C语言调用保护
;=================================================
; POPD *+
; SAR AR0,*+
; SAR AR1,*
; LARK AR0,#1
; LAR AR0,*0+,AR0
;========================================================================
; 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
;========================================================================
; park transformation
; isd=foc_isalfa*cosTeta_cm+foc_isbeta*sinTeta_cm
; isq=-foc_isalfa*sinTeta_cm+foc_isbeta*cosTeta_cm
;========================================================================
dq_change:
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
;=================================================
;调用完毕，跳出
;=================================================
; SBRK #2
; LAR AR0,*-
; PSHD *
ret
;===========================================================================
; inverse park transformation
; vsbeta_ref=vsq_ref*cosTeta_cm+vsd_ref*sinTeta_cm
; vsalfa_ref=-vsq_ref*sinTeta_cm+vsd_ref*cosTeta_cm
;===========================================================================
_park_inv:
;=================================================
;C语言调用保护
;=================================================
; POPD *+
; SAR AR0,*+
; SAR AR1,*
; LARK AR0,#1
; LAR AR0,*0+,AR0
;==================================================
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
;=================================================
;调用完毕，跳出
;=================================================
; SBRK #2
; LAR AR0,*-
; PSHD *
ret
_Sincos:
;=================================================
;C语言调用保护
;=================================================
; POPD *+
; SAR AR0,*+
; SAR AR1,*
; LARK AR0,#1
; LAR AR0,*0+,AR0
;=================================================
spm 0
setc sxm
ldpk _thet
lt _thet
mpy _thet_count
spm 0
pac
sacl _thet1
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -