dc-pi.asm

直流电机驱动程序

           .title "dc-pi.asm"
           .bss _speed_err,1
           .bss _speed_err_old,1
           .bss _speed_kp,1
           .bss _current_given,1
           .bss _speed_ki,1
           .bss _speed_kd,1
           .bss _speed_pi_error,1
           .bss _speed_pi_error1,1
           .bss _speed_sum,1
           .bss _pi_temp,1
           .global _speed_err,_current_given,_speed_sum
           .global _current_err,_current_uref,_current_sum
           
           .bss _current_err,1
           .bss _current_err_old,1
           .bss _current_kp,1
           .bss _current_uref,1
           .bss _current_ki,1
           .bss _current_kd,1
           .bss _current_pi_error,1
           .bss _current_pi_error1,1
           .bss _current_sum,1

           .global _speed_pi_loop,_speed_kp,_speed_ki,_speed_kd
           .global _current_pi_loop,_current_kp,_current_ki,_current_kd
           
           .def _speed_pi_loop,_current_pi_loop    

;================================================
;速度环
;================================================                        
_speed_pi_loop:
;=================================================
          POPD        *+
          SAR        AR0,*+
          SAR        AR1,*
          LARK       AR0,#1
          LAR        AR0,*0+,AR0
;=================================================          
          
          setc SXM
speed_pi_p:         
          ldpk  _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
          
neg_u_limit:
          SPLK  #-500,_current_given               
speed_over:
          lacc  _speed_err
          sacl  _speed_err_old
          
;================================================
          SBRK       #2
          LAR        AR0,*-
          PSHD        *
          ret   
;================================================
;电流环
;================================================          
_current_pi_loop:
;================================================
          POPD        *+
          SAR        AR0,*+
          SAR        AR1,*
          LARK       AR0,#1
          LAR        AR0,*0+,AR0
;================================================          
          setc  SXM
          
current_pi_p:
          ldpk  _current_err
          lt    _current_err
          mpy   _current_kp
          spm   3
          pac      
          sfr
          sfr    
          sacl   _current_uref          
current_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     current_pi_d

Buck_bb:          
          lacc  _current_sum 
          rpt   #2
          sfr      
          add   _current_uref 
          sacl  _current_uref 
          
current_pi_d:
          lacc  _current_err
          sub   _current_err_old
          sacl  _pi_temp
          lt    _pi_temp
          mpy   _current_kd
          spm   3
          pac             
          sfr
          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:
          lacc  _current_err
          sacl  _current_err_old
;================================================
          SBRK       #2
          LAR        AR0,*-
          PSHD        *
          ret