pid_reg2.asm

TI的digital motor control lib的源代码。了解TI的编程规范

                BCND  __PID2_Sat_Minus_Io, LT
                                ; If (upi_reg2 + min_reg2) <  0 go to label 
                                ; __PID2_Sat_Minus_Io
                                ; ARP = AR3. AR3 -> min_reg2
;--------------------------------------------------------------------------------        
                SBRK  #3        ; AR3 -> upi_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------            
                LACC  *         ; ACC[Q15] = upi_reg2[Q15]
                                ; AR3 -> upi_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------            
                ADRK  #2        ; AR3 -> max_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------            
                SUB   *         ; ACC = upi_reg2 - max_reg2
                                ; ARP = AR3
                                ; AR3 -> max_reg2
                                ; ARP = AR3.
;--------------------------------------------------------------------------------            
                BCND  __PID2_Sat_Plus_Io, GEQ
                                ; If (upi_reg2 - max_reg2) >= 0 go to 
                                ; label __PID2_Sat_Plus_Io
                                ; AR3 -> max_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------                                
                SBRK  #2        ; AR3 -> upi_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------            
                LACC  *         ; ACC = upi_reg2
                                ; ARP = AR3. AR3 -> upi_reg2
;--------------------------------------------------------------------------------            
                ADRK  #4        ; AR3 -> out_reg2
                                ; ARP = AR3
;--------------------------------------------------------------------------------            
                SACL  *         ; out_reg2[Q15] = upi_reg2[Q15]
                                ; AR3 -> out_reg2
                                ; ARP = AR3
;--------------------------------------------------------------------------------            
                B     __PID2_Fwd_Io    
                                ; AR3 -> out_reg2
                                ; ARP = AR3
;--------------------------------------------------------------------------------            
            ;Saturation control
__PID2_Sat_Minus_Io:
                                ; AR3 -> out_reg2
                LACC  *+        ; ACC = min_reg2. 
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------            
                SACL  *         ; out_reg2[Q15] = min_reg2[Q15]
                                ; ARP = AR3. 
                                ; AR3 -> out_reg2
;--------------------------------------------------------------------------------            
                B     __PID2_Fwd_Io    
                                ; ARP = AR3. 
                                ; AR3 -> out_reg2
;--------------------------------------------------------------------------------            
__PID2_Sat_Plus_Io:    
                                ; AR3 -> max_reg2
                LACC  *         ; ACC = max_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------            
                ADRK  #2        ; AR3 -> out_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------                        
                SACL  *         ; out_reg2[Q15] = max_reg2[Q15]
                                ; AR3 -> out_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------    
__PID2_Fwd_Io:    
                                ; AR3 -> out_reg2
                LACC  *         ; ACC = out_reg2[Q15]
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------            
                SBRK  #4        ; AR3 -> upi_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------    
                SUB   *+        ; ACC = out_reg2[Q15] - upi_reg2[Q15]
                                ; ARP = AR3. AR3 -> epi_reg2
;--------------------------------------------------------------------------------    
                                ; AR3 -> epi_reg2
                SACL  *         ; epi_reg2 = out_reg2 - Upi_reg2
                                ; ARP = AR3
;--------------------------------------------------------------------------------    
                LT    *         ; TREG = epi_reg2[Q15]
                                ; ARP = AR3. AR3 -> epi_reg2
;--------------------------------------------------------------------------------    
                SBRK  #4        ; AR3 -> kc_reg2[Q13]
                                ; ARP = AR3
;--------------------------------------------------------------------------------    
                MPY   *         ; PREG[Q28] = kc_reg2[Q13] * epi_reg2[Q15]
                                ; ARP = AR3
                                ; AR3 -> kc_reg2[Q13]
;--------------------------------------------------------------------------------    
                PAC             ; ACC[Q28] = PREG[Q28]
                                ; ARP = AR3
                                ; AR3 -> kc_reg2[Q13]
;--------------------------------------------------------------------------------    
                ADRK  #2        ; AR3 -> en0_reg2
                                ; ARP = AR3
;--------------------------------------------------------------------------------    
                LT    *         ; TREG -> en0_reg2[Q15]
                                ; AR3 -> en0_reg2
                                ; ARP = AR3
;--------------------------------------------------------------------------------    
                SBRK  #3        ; AR3 -> k1_reg2[Q13]
                                ; ARP = AR3
;--------------------------------------------------------------------------------    
                MPY   *         ; PREG[Q28] = k1_reg2[Q13] * en0_reg2[Q15]
                                ; AR3 -> k1_reg2[Q13]
                                ; ARP = AR3
;--------------------------------------------------------------------------------    
                APAC            ; ACC[Q28] = K1_reg2[Q13] * En0_reg2[Q15])[Q28]
                                ; + (Kc_reg2[Q13] * epi_reg2[Q15])[Q28]
                                ; AR3 -> k1_reg2[Q13]
                                ; ARP = AR3
;--------------------------------------------------------------------------------    
                ADRK  #2        ; AR3 -> un_reg2
                                ; ARP = AR3
;--------------------------------------------------------------------------------    
                ADD   *, 13     ; ACC[Q28] = K1_reg2[Q13] * En0_reg2[Q15])[Q28]
                                ; + (Kc_reg2[Q13] * epi_reg2[Q15])[Q28] + 
                                ; (un_reg2[Q15] << 13)[Q28]
                                ; AR3 -> un_reg2
                                ; ARP = AR3
;--------------------------------------------------------------------------------    
                RPT   #2
                  NORM *        ; ACC[Q31].  
                NOP             ; ARP = AR3. AR3 -> un_reg2
                NOP             ; NOP is used to avoid "pipe line conflict"
;--------------------------------------------------------------------------------         
        
                SACH  *, AR1    ; un_reg2[Q15] = (Higher order 16 bits of 
                                ; ACC[Q31])[Q15]
                                ; ARP = AR1
                                
                ; Note: ARP = AR1 in preparation for exit.                
;================================================================================
__pid2_calc_exit:
    
    SBRK  #(__PID2_framesize + 1)
                                ; deallocate frame. Point to saved FP
                                ; ARP = AR1
    LAR   AR0, *-               ; Restore frame pointer. ARP = AR1
    PSHD  *                     ; Push return address on Hardware stack. ARP = AR1
    RET                         ; Return to the parent function. ARP = AR1
  
    .end