pi.asm

永磁同步电机驱动控制

           .title "pi.asm"
_temp               .usect  ".blk1",  1             
_temp_h             .usect  ".blk1",  1    
_temp_l             .usect  ".blk1",  1           
_n_ref              .usect  ".blk1",  1
_n_feed             .usect  ".blk1",  1
_t23                .usect  ".blk1",  1
_t22                .usect  ".blk1",  1
_pulse_number       .usect  ".blk1",  1
_speed_count        .usect  ".blk1",  1
_speed_err 			.usect  ".blk1",  1 
_speed_kp           .usect  ".blk1",  1
_current_given      .usect  ".blk1",  1
_speed_ki           .usect  ".blk1",  1
_speed_pi_error     .usect  ".blk1",  1
_speed_pi_error1    .usect  ".blk1",  1
_speed_sum          .usect  ".blk1",  1
           
_current_err        .usect  ".blk1",  1
_current_kp     	.usect  ".blk1",  1
_current_uref       .usect  ".blk1",  1
_current_ki   	    .usect  ".blk1",  1
_current_pi_error   .usect  ".blk1",  1
_current_pi_error1  .usect  ".blk1",  1
_current_sum        .usect  ".blk1",  1
           
           .global _temp_h,_temp_l,_temp,_n_feed
           .global _n_ref,_speed_count,_t23
           .global _speed_err,_current_given,_speed_sum
           .global _current_err,_current_uref,_current_sum
           .global _speed_pi_error,_speed_pi_error1
           .global _current_pi_error,_current_pi_error1
               
           .def _speed_pi_on,_current_cal_pi
           .global _speed_pi_on,_speed_kp,_speed_ki
           .global _current_cal_pi,_current_kp,_current_ki
                                       
_speed_pi_on:
            setc SXM
            ldpk    _t23
            lacc    _t23
            sub     _t22       
            ;neg     
            sacl    _pulse_number
            lacc    _t23
            sacl    _t22
            
            lt      _pulse_number              ;瞬时速度计算
            mpy     _speed_count
            spm     3
            pac     
            sfr
            sfr                     
            sacl     _n_feed   

             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     lianghua2  

pip1:    
     
          spm   3
          pac         
          sfr
          sfr
          sfr    
          sfr   
          sfr   
          add   _speed_sum
          sacl  _speed_sum

lianghua2:  
        
          lacc  _speed_sum
          blz   neg_limit_speed   
          abs         
          sub   #8000
          blz   yg2_speed                
          lacc  #8000
          sacl  _speed_sum        
          b     yg2_speed
          
neg_limit_speed: 
;          sub   #0F05FH              ;?
;          BLZ   L1                   ;?
          splk  #0,_speed_sum        
;          B     yg2_speed            ;?
          
;L1:       SPLK  #0F05FH, _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_limit

pip3:          
          lacc  _speed_sum    
          rpt   #2
          sfr      
          add  _current_given
          sacl _current_given

speed_pi_limit:

          lacc  _current_given
          blz   neg_u_limit
          abs
          sub   #1000
          BLZ   speed_over           
          lacc  #1000
          sacl  _current_given 
          
          B     speed_over
          
neg_u_limit:
;          lacc  _current_given           ;?      
;          sub   #0FE0BH                  ;?
;          BLZ   LL                       ;?
          SPLK  #0,_current_given   
;          B     speed_over               ;?
          
;LL:       SPLK  #0FE0BH, _current_given  ;?             
speed_over:
          
          ret   
          
_current_cal_pi:
          setc  SXM
          ldpk  _current_err
          lt    _current_err
          mpy   _current_kp
          spm   3
          pac      
          sfr
          sfr    
          sacl   _current_uref          
Buck_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_sum
          sacl  _current_sum
          b     pi_onit1 

Buck_aa:
          spm   3
          pac
          sfr
          sfr   
          sfr
          sfr     
          sfr
          add   _current_sum
          sacl  _current_sum
                    
pi_onit1:
          lacc  _current_sum
          blz   no_neg
          abs
          sub   #8000
          blz   yg2               
          splk  #8000,_current_sum                    
          b     yg2
no_neg:  
          splk  #0,_current_sum
yg2:       
          lacc  _current_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_sum 
          rpt   #2
          sfr
          add   #1
          add   _current_uref 
          sacl  _current_uref 
          b     Buck_pi_limit

Buck_bb:          
          lacc  _current_sum 
          rpt   #2
          sfr      
          add   _current_uref 
          sacl  _current_uref 

Buck_pi_limit:
          lacc  _current_uref 
          blz   Buck_u_limit
          abs
          sub  #1000
          BLZ   Buck_pwm_over        ;限幅值
          SPLK  #1000,_current_uref 
          B     Buck_pwm_over
          
Buck_u_limit:
          SPLK  #0,_current_uref 
Buck_pwm_over:
          lacc  _n_ref
          bnz   back 
          splk  #0,_speed_pi_error
          splk  #0,_speed_pi_error1
          splk  #0,_speed_sum
          splk  #0,_current_given
          
          splk  #0,_current_pi_error
          splk  #0,_current_pi_error1
          splk  #0,_current_sum
          splk  #0,_current_uref
back:          
          ret