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