arithmetic.asm

永磁同步电机驱动控制

                .include "sintable.h"
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

           .global _temp_h,_temp_l,_temp,_t23
;===========================================================
_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
_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,_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

           .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
           .def _arithmetic_foc
           .global _arithmetic_foc
;=================================================================

;================================================
;变量初始化
;================================================
_variable_initial:
            ldpk _n_ref
            
            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 #40,_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_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 #10,_current_kp
            splk #0,_current_uqref
            splk #5,_current_ki
            splk #5,_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 #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
           
           ret
;================================================
;FOC算法
;================================================                 
_arithmetic_foc:
          call  _speed_pi_loop 
          call  _Sincos    
          call  _current_clarke
          call  _current_pi_loop
          call  _park_inv
          call  _swpwm_modulation
          ret
;================================================
;速度环
;================================================                        
_speed_pi_loop:
;=================================================
;          POPD        *+
;          SAR        AR0,*+
;          SAR        AR1,*
;          LARK       AR0,#1
;          LAR        AR0,*0+,AR0
;=================================================          
          ldpk    _t23
          setc    SXM
          lacc    _t23
          sub     _t22       
          ;neg     
          sacl    _t2_add
          lacc    _t23
          sacl    _t22

          lacc  _speed_number
          sub   #1
          sacl  _speed_number
          bnz    next_speed_loop
          
          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   #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