📄 arithmetic.asm
字号:
.include "sintable.h"
pwmprd .set 500 ;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
;===========================================================
_speed_given .usect ".blk3",1
_speed_test .usect ".blk3",1
_speed_err .usect ".blk3",1
_speed_err_old .usect ".blk3",1
_speed_kp .usect ".blk3",1
_current_given .usect ".blk3",1
_speed_ki .usect ".blk3",1
_speed_kd .usect ".blk3",1
_speed_pi_error .usect ".blk3",1
_speed_pi_error1 .usect ".blk3",1
_speed_sum .usect ".blk3",1
_pi_temp .usect ".blk3",1
_t2_sum .usect ".blk3",1
_speed_count .usect ".blk3",1
.global _speed_given,_speed_test,_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
_current_iq_err .usect ".blk3",1
_current_iq_err_old .usect ".blk3",1
_current_kp .usect ".blk3",1
_current_uqref .usect ".blk3",1
_current_ki .usect ".blk3",1
_current_kd .usect ".blk3",1
_current_pi_error .usect ".blk3",1
_current_pi_error1 .usect ".blk3",1
_current_iq_sum .usect ".blk3",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 ".blk3",1
_current_id_err_old .usect ".blk3",1
_current_udref .usect ".blk3",1
_current_pi_error2 .usect ".blk3",1
_current_pi_error3 .usect ".blk3",1
_current_id_sum .usect ".blk3",1
.global _current_id_err,_current_id_err_old,_current_udref,_current_pi_error2
.global _current_pi_error3,_current_id_sum
;==================================================================
_ia .usect ".blk3",1
_ib .usect ".blk3",1
_sampling_k .usect ".blk3",1
_is_alfa .usect ".blk3",1
_is_beta .usect ".blk3",1
_isd .usect ".blk3",1
_isq .usect ".blk3",1
_is_temp .usect ".blk3",1
_vsbeta_ref .usect ".blk3",1
_vsalfa_ref .usect ".blk3",1
.global _ia,_ib,_sampling_k,_is_alfa
.global _is_beta,_isd,_isq,_is_temp
.global _vsbeta_ref,_vsalfa_ref
;==================================================================
_thet .usect ".blk3",1
_thet1 .usect ".blk3",1
_s_sin .usect ".blk3",1
_s_cos .usect ".blk3",1
_ang_cos .usect ".blk3",1
_ang_sin .usect ".blk3",1
_sin_temp .usect ".blk3",1
_sin_value .usect ".blk3",1
_cos_value .usect ".blk3",1
.global _thet,_thet1,_s_sin,_s_cos
.global _ang_cos,_ang_sin,_sin_temp,_sin_value
.global _cos_value
;=================================================================
_vref1 .usect ".blk3",1
_vref2 .usect ".blk3",1
_vref3 .usect ".blk3",1
_vref4 .usect ".blk3",1
_vref5 .usect ".blk3",1
_x .usect ".blk3",1
_y .usect ".blk3",1
_z .usect ".blk3",1
_a .usect ".blk3",1
_b .usect ".blk3",1
_c .usect ".blk3",1
_sector .usect ".blk3",1
_t1 .usect ".blk3",1
_t2 .usect ".blk3",1
_taon .usect ".blk3",1
_tbon .usect ".blk3",1
_tcon .usect ".blk3",1
_cmp1 .usect ".blk3",1
_cmp2 .usect ".blk3",1
_cmp3 .usect ".blk3",1
_tmp .usect ".blk3",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 _swpwm_modulation
.global _swpwm_modulation
.def _variable_initial
.global _variable_initial
;=================================================================
;================================================
;变量初始化
;================================================
_variable_initial:
ldpk _speed_given
splk #600h,_speed_given
out _speed_given,0ffffh
splk #0,_speed_given
splk #0,_speed_test
splk #0,_speed_err
splk #0,_speed_err_old
splk #40,_speed_kp
splk #0,_current_given
splk #2,_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_sum
splk #18750,_speed_count ;Q8,73.2421 300=60r/m
splk #0,_current_iq_err
splk #0,_current_iq_err_old
splk #40,_current_kp
splk #0,_current_uqref
splk #40,_current_ki
splk #40,_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 #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
;================================================
;速度环
;================================================
_speed_pi_loop:
;=================================================
; POPD *+
; SAR AR0,*+
; SAR AR1,*
; LARK AR0,#1
; LAR AR0,*0+,AR0
;=================================================
setc SXM
speed_pi_p:
ldpk _t2_sum
lt _t2_sum
mpy _speed_count
spm 3
pac
sfr
sfr
sacl _speed_test
lacc _speed_given
sub _speed_test
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
;================================================
; SBRK #2
; LAR AR0,*-
; PSHD *
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 #4624
blz yg1
lacc _current_iq_sum
blz no_neg
splk #4624,_current_iq_sum
b yg1
no_neg:
splk #-4624,_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 #578
BLZ Buck_pwm_over ;限幅值
lacc _current_uqref
blz Buck_u_limit
SPLK #578,_current_uqref
B Buck_pwm_over
Buck_u_limit:
SPLK #-578,_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
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -