pi.asm

直流电机驱动程序

           .title "pi.asm"
           .bss _speed_err,1
           .bss _speed_kp,1
           .bss _current_given,1
           .bss _speed_ki,1
           .bss _speed_pi_error,1
           .bss _speed_pi_error1,1
           .bss _speed_sum,1
           
           .bss _current_err,1
           .bss _current_kp,1
           .bss _current_uref,1
           .bss _current_ki,1
           .bss _current_pi_error,1
           .bss _current_pi_error1,1
           .bss _current_sum,1
           .global _speed_err,_current_given,_speed_sum
           .global _current_err,_current_uref,_current_sum
               
           .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
         
          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   no_neg_speed   
          abs         
          sub   #4000
          blz   yg2_speed 
          lacc  _speed_sum
          blz   neg_limit_speed                
          lacc  #4000
          sacl  _speed_sum        
          b     yg2_speed
          
neg_limit_speed: 
;          sub   #0F05FH              ;?
;          BLZ   L1                   ;?
          splk  #-4000,_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
          abs
          sub   #500
          BLZ   speed_over           
          lacc  _current_given
          blz   neg_u_limit
          lacc  #500
          sacl  _current_given 
          
          B     speed_over
          
neg_u_limit:
;          lacc  _current_given           ;?      
;          sub   #0FE0BH                  ;?
;          BLZ   LL                       ;?
          SPLK  #-500,_current_given   
;          B     speed_over               ;?
          
;LL:       SPLK  #0FE0BH, _current_given  ;?             
speed_over:
          
          ret   
          
_current_cal_pi:
          setc  SXM

          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
          abs
          sub   #4000
          blz   yg2
          lacc  _current_sum
                     
          
          blz   no_neg                
          splk  #4000,_current_sum                    
          b     yg2
no_neg:  
          splk  #-4000,_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 
          abs
          sub  #500
          BLZ   Buck_pwm_over        ;限幅值
          lacc  _current_uref
         
          blz  Buck_u_limit
          SPLK  #500,_current_uref 
          B     Buck_pwm_over
Buck_u_limit:
          SPLK  #-500,_current_uref 
Buck_pwm_over:
          ret