?? asy_motor_control.asm
字號:
.include "sintable.h"
.include "led.asm"
;-----------------------------------------------------------
T2CNT .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
_ic .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,_ic,_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
_init_counter .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
;-----------------------------------------------------------
_imr .usect ".blk1", 1
_foc_temp .usect ".blk1", 1
_foc_temp1 .usect ".blk1", 1
_kr .usect ".blk1", 1
_kt .usect ".blk1", 1
_k .usect ".blk1", 1
_cur_error .usect ".blk1", 1
_cur_error1 .usect ".blk1", 1
_cur_error2 .usect ".blk1", 1
_fs .usect ".blk1", 1
_tetaincr .usect ".blk1", 1
.global _imr,_foc_temp,_foc_temp1
.global _kr,_kt,_k
.global _cur_error,_cur_error1,_cur_error2
.global _fs,_tetaincr
;----------------------------------------------------------
; .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 _asy_motor_control
cmpr_value .usect ".blk1", 1
temp1 .usect ".blk1", 1
ar4_temp .usect ".blk1", 1
ar6_temp .usect ".blk1", 1
;-----------------------------------------------------------------
;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:
led_initial:
ldpk confirm ;clear sample registers
splk #0,confirm
splk #0,key_read_time
splk #0,key_temp
splk #0,point_flag
splk #0,led_pos
splk #0,pos
splk #1,flash_num
splk #1,show_num
splk #0,flash_code
splk #0,led_send_flag
splk #0,temp_high
splk #0,temp_low
splk #0,buffer
splk #0,led_data
splk #0,set_data
splk #10,count_value ;常數
splk #10000,count_value2 ;常數3
splk #19h, led01 ;p
splk #0fh, led02 ;S
splk #018h, led03 ;d
splk #01ah, led04 ;_
splk #0Ch, led05 ;H
splk #017h, led06 ;T
splk #0, ledcode01
splk #0, ledcode02
splk #0, ledcode03
splk #0, ledcode04
splk #0, ledcode05
splk #0, ledcode06
splk #0, ledcodeflash01
splk #0, ledcodeflash02
splk #0, ledcodeflash03
splk #0, ledcodeflash04
splk #0, ledcodeflash05
splk #0, ledcodeflash06
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 #5,_speed_ki
splk #5,_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 #40,_current_kp
splk #0,_current_uqref
splk #20,_current_ki
splk #20,_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 #0,_ic
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
splk #0,_imr
splk #35,_kr ;35
splk #2000,_kt ;2000
splk #18,_k ;18
splk #0,_cur_error
splk #0,_cur_error1
splk #0,_cur_error2
splk #0,_foc_temp
splk #0,_foc_temp1
splk #0,_fs
splk #0,_tetaincr
lar ar4,#0a000h
lar ar6,#0a800h
splk #1,cmpr_value
splk #0a000h,ar4_temp
splk #0a800h,ar6_temp
splk #30000,_init_counter
ret
;=====================================================
;FOC算法
;=====================================================
_asy_motor_control:
; b start_run
ldpk _init_phase_flag
lacc _init_phase_flag
bcnd start_run,neq
splk #400,_cmp1
splk #500,_cmp2
splk #500,_cmp3
lacc _init_counter
sub #1
sacl _init_counter
bcnd back,neq
splk #1,_init_phase_flag
splk #30000,_init_counter
start_run:
;==========================================================================
; .if sin_cos ;sin and cos cal
;==========================================================================
setc sxm
ldpk _thet
lacc _thet
;mpy _thet_count
;spm 0
;pac
sacl _thet1
ppt_aa:
lacc _thet1
abs
sub #A360
blz onit
lacc _thet1
blz neg_limit11
sub #A360
sacl _thet1
b ppt_aa
neg_limit11:
lacc _thet1
add #A360
sacl _thet1
b ppt_aa
onit:
lacc _thet1
bgz next_ca
add #A360
sacl _thet1
next_ca:
lacc _thet1
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 current_clarke ;CLARKE變換
;========================================================================
; clarke transformation
; i_alfa=ia;
; i_beta=(1/sqrt(3))*(2*ib+ia);
;========================================================================
; setc SXM
ldpk _ic
lacc _ic
neg
sacl _ic
lacc _ib
neg
sacl _ib
lacc _ic ;這里a/c
add _ib
neg
sacl _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
lacc #0
lt _is_beta
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -