arithmetic.asm

永磁同步电机驱动控制

        lacc   _thet1
        
        bgz  	onit
        add  	#A360
        sacl 	_thet1
onit:

        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
;=================================================
;调用完毕，跳出
;=================================================        
;          SBRK       #2
;          LAR        AR0,*-
;          PSHD        *
          ret         
               
;===========================================================================
;svpwm modulation
;===========================================================================
_swpwm_modulation:
;=================================================
;C语言调用保护
;=================================================
;          POPD        *+
;          SAR        AR0,*+
;          SAR        AR1,*
;          LARK       AR0,#1
;          LAR        AR0,*0+,AR0
;================================================
         setc SXM
        ldpk    _vsbeta_ref
        lacc  	_vsbeta_ref   
        sacl  	_vref1
        lt    	_vsalfa_ref
        mpy   	#sqrt32
        spm   	0
        pac 
        sach  	_vref4,4
        lacc  	_vsbeta_ref 
        sfr
        neg
        sacl  	_vref5
        add   	_vref4
        sacl  	_vref2
        lacc  	_vref5
        sub   	_vref4
        sacl  	_vref3
         
        lacc  	_vref1
        sacl  	_x
        lacc  	_vref4
        sub   	_vref5
        sacl  	_y
        lacc  	_vref4
        add   	_vref5
        neg
        sacl  	_z
             
             
        lacc  	_vref1
        bgz   	ua
        splk  	#0,_a
        b     	uaon
ua:
        splk  	#1,_a
              
uaon:
        lacc  	_vref2
        bgz   	ub
        splk   	#0,_b
        b     	ubon
ub:
        splk  	#1,_b
ubon:
        lacc 	_vref3
        bgz   	uc
        splk  	#0,_c
        b  		ucon
uc:
        splk  	#1,_c
ucon:   
        lacc  	_c,2
        add  	_b,1
        add  	_a 
        sacl  	_sector
        sub  	#1
        bz   	f_sector1
        sub  	#1
        bz   	f_sector2
        sub   	#1
        bz   	f_sector3
        sub   	#1
        bz   	f_sector4
        sub   	#1
        bz   	f_sector5
        sub   	#1   
        bz   	f_sector6
        b    	cmp_ok
f_sector1:
        lacc  	_z
        sacl  	_t1
        lacc  	_y
        sacl  	_t2
        
        b    	t1t2out 
           
f_sector2:
        lacc 	_y
        sacl 	_t1
        lacc  	_x
        neg
        sacl  	_t2
    
        b    	t1t2out   
            
f_sector3:
        lacc  	_z
        neg
        sacl 	_t1
        lacc  	_x
              
        sacl 	_t2
   
        b    	t1t2out  
          

                            
f_sector4:
        lacc  	_x
        neg
        sacl  	_t1
        lacc  	_z
        sacl  	_t2 
   
        b    	t1t2out 
f_sector5:
        lacc   	_x
             
        sacl  	_t1
        lacc  	_y
        neg
        sacl  	_t2
 
        b    	t1t2out 
          
f_sector6:
        lacc 	_y
        neg
        sacl  	_t1
        lacc  	_z
        neg
        sacl  	_t2
        
t1t2out:     
        b   	t1t2_ok

        lt   	_t1
        mpy  	#pwmprd
        spm  	1
        pac
              
        sach  	_t1,3
 
        lt  	_t2
        mpy   	#pwmprd
        spm  	1
        pac 
              
        sach  	_t2 ,3
t1t2_ok:              
        lacc   	_t1
        bgz   	t1_ok
        splk   	#0,_t1
t1_ok:
        lacc  	_t2
        bgz   	t2_ok
        splk   	#0,_t2
t2_ok:
        lacc   	_t1
        add    	_t2
        sacl   	_tmp
        sub    	#pwmprd
        blez   	nosaturation
             
              
        lacc  	#pwmprd,15
        rpt   	#15
        subc  	_tmp
        sacl  	_tmp
        lt    	_tmp
        mpy   	_t1
        spm    	0
        pac 
        sach  	_t1,1
        mpy   	_t2
        pac
        sach  	_t2,1
nosaturation:
        lacc  	#pwmprd
        sub   	_t1
        sub   	_t2
        sfr
        sacl  	_taon
        add  	_t1
        sacl  	_tbon
        add  	_t2
        sacl  	_tcon
              
        lacc  	_sector
        sub   	#1
        bnz   	nosect1
        lacc   	_tbon
        sacl   	_cmp1
        lacc   	_taon
        sacl   	_cmp2
        lacc   	_tcon
        sacl   	_cmp3
        b      	cmp_ok
              
nosect1:
        sub  	#1
        bnz   	nosect2
        lacc  	_taon
        sacl  	_cmp1
        lacc  	_tcon
        sacl  	_cmp2
        lacc   	_tbon
        sacl   	_cmp3
        b   	cmp_ok
                                                                    
nosect2:
        sub   	#1
        bnz   	nosect3
        lacc  	_taon
        sacl  	_cmp1
        lacc  	_tbon
        sacl  	_cmp2
        lacc   	_tcon
        sacl  	_cmp3
        b   	cmp_ok

nosect3:
        sub   	#1
        bnz   	nosect4
        lacc  	_tcon
        sacl   	_cmp1
        lacc   	_tbon
        sacl   	_cmp2
        lacc   	_taon
        sacl   	_cmp3
        b   	cmp_ok
        
nosect4:
        sub   	#1
        bnz   	nosect5
        lacc  	_tcon
        sacl  	_cmp1
        lacc   	_taon
        sacl   	_cmp2
        lacc   	_tbon
        sacl   	_cmp3
        b    	cmp_ok
            

nosect5:
        lacc   	_tbon
        sacl  	_cmp1
        lacc   	_tcon
        sacl   	_cmp2
        lacc  	_taon
        sacl 	_cmp3  
        
cmp_ok:
        lacc  	#pwmprd          ;反向逻辑需要进行以下修正
        sub    	_cmp1
        sacl   	_cmp1  
        lacc   	#pwmprd
        sub    	_cmp2
        sacl   	_cmp2
        lacc   	#pwmprd
        sub    	_cmp3
        sacl   	_cmp3  
;================================
;这里要考虑实际输出的矢量与电机正
;方向的关系，来决定上述是否要减
;================================        
        lacc  _n_ref
        bnz    back
            
            splk #0,_speed_err
            splk #0,_speed_err_old
            splk #0,_current_given
            splk #0,_speed_pi_error
            splk #0,_speed_pi_error1
            splk #0,_speed_sum
            splk #0,_t2_sum

            splk #0,_current_iq_err
            splk #0,_current_iq_err_old
            splk #0,_current_uqref
            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,_thet
            splk #0,_thet1
            
            splk #0,_cmp1
            splk #0,_cmp2
            splk #0,_cmp3
;=================================================
;调用完毕，跳出
;=================================================        
;          SBRK       #2
;          LAR        AR0,*-
;          PSHD        *
back:
            ret