📄 asy_motor_control.asm
字号:
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变换结束
;==============================================
;current model
;==============================================
current_model:
lacc _isd
sub _imr
sacl _foc_temp
lt _foc_temp
mpy _kr
pac
clrc SXM
and #07fffh
add _cur_error
sacl _cur_error
sub #32768
blz no_limit1
sacl _cur_error
setc SXM
pac
sach _foc_temp,1
lacc _foc_temp
add #1
sacl _foc_temp
add _imr
sacl _imr
bcnd i_mrnotzero,neq
lacc #0
sacl _foc_temp
b i_mrzero
no_limit1:
setc SXM
pac
sach _foc_temp,1
lacc _foc_temp
add _imr
sacl _imr
bcnd i_mrnotzero,neq
lacc #0
sacl _foc_temp
b i_mrzero
i_mrnotzero:
lacc _imr
blz i_mrnotzero2
bcnd i_mrzero,eq
sacl _foc_temp1
lacc _isq
abs
sacl _foc_temp
lacc _foc_temp,12
rpt #15
subc _foc_temp1
sacl _foc_temp
lacc _isq
bcnd isqpos,gt
lacc _foc_temp
neg
sacl _foc_temp
b isqpos
i_mrnotzero2:
abs
sacl _foc_temp1
lacc _isq
abs
sacl _foc_temp
lacc _foc_temp,12
rpt #15
subc _foc_temp1
sacl _foc_temp
lacc _isq
bcnd isqpos,lt
lacc _foc_temp
neg
sacl _foc_temp
isqpos
i_mrzero
lt _foc_temp
mpy _kt
pac
clrc SXM
and #0fffh
add _cur_error1
sacl _cur_error1
sub #4096
blz no_limit2
sacl _cur_error1
setc SXM
pac
sach _foc_temp,4
lacc _foc_temp
add #1
sacl _foc_temp
add _n_feed
sacl _fs
b onitcur1
no_limit2:
setc SXM
pac
sach _foc_temp,4
lacc _foc_temp
add _n_feed
sacl _fs
onitcur1:
lacc _fs
abs
sacl _foc_temp
lt _foc_temp
mpy _k
pac
pac
clrc SXM
and #0fffh
add _cur_error2
sacl _cur_error2
sub #4096
blz no_limit3
sacl _cur_error2
setc SXM
pac
sach _tetaincr,4
lacc _tetaincr
add #1
sacl _tetaincr
bit _fs,0
bcnd fs_neg,tc
lacc _tetaincr
adds _thet
sacl _thet
b fs_pos
no_limit3:
setc SXM
pac
sach _tetaincr,4
bit _fs,0
bcnd fs_neg,tc
lacc _tetaincr
adds _thet
sacl _thet
b fs_pos
fs_neg
lacc _thet
subs _tetaincr
sacl _thet
fs_pos:
lacc _thet
abs
sub #A360
blz speed_loop
lacc _thet
blz neglimit
sub #A360
sacl _thet
b fs_pos
neglimit:
add #A360
sacl _thet
b fs_pos
;===========================================================================
; .if speed_regulator ;速度环
;====================================================
speed_loop:
; ldpk _t23
setc SXM
lacc _t23
sub _t22
neg
sacl _t2_add
lacc _t23
sacl _t22
;splk #1,_t2_add
lacc _speed_number
sub #1
sacl _speed_number
bnz speed_over_1
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 #1600
blz yg2_speed
lacc _speed_sum
blz neg_limit_speed
lacc #1600
sacl _speed_sum
b yg2_speed
neg_limit_speed:
splk #-1600,_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 #200
BLZ speed_over
lacc _current_given
blz neg_u_limit
lacc #200
sacl _current_given
B speed_over
neg_u_limit:
SPLK #-200,_current_given
speed_over:
lacc _speed_err
sacl _speed_err_old
speed_over_1:
lacc _t2_sum
add _t2_add
sacl _t2_sum
; .endif ;speed loop over
;================================================================
init:
;================================================================
; .if iq_requlator ;iq电流调节
;================================================================
;setc SXM
;b u_park
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 #3200
blz yg1
lacc _current_iq_sum
blz no_neg
splk #3200,_current_iq_sum
b yg1
no_neg:
splk #-3200,_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 #400
BLZ Buck_pwm_over ;限幅值
lacc _current_uqref
blz Buck_u_limit
SPLK #400,_current_uqref
B Buck_pwm_over
Buck_u_limit:
SPLK #-400,_current_uqref
Buck_pwm_over:
lacc _current_iq_err
sacl _current_iq_err_old
; .endif ;q轴电流调节结束
;---------------------------------------------------------------------
;=====================================================
; .if id_requlator ;d轴电流调节
;=====================================================
current_pi_id_p:
; b u_park
;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
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -