pid_reg2.asm

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

;=====================================================================================
; File name:        pid_reg2.asm                     
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:                                 
; PID Controller with anti-windup
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx.  (Peripheral Independant).
;
;
;================================================================================
; Routine Name: pid2_calc                  Routine Type: C Callable
;
; Description:
;  
; C prototype : void  pid2_calc(PID2 *p)
;================================================================================
; History     Created  July 18,2000
;
; Definition of PID2:
;
; typedef struct PID2 {
;
;    int fb_reg2;    /* Feedback signal for PI regulator Q15 Input  */
;    int ref_reg2;   /* Reference signal for PI regulator Q15 Input */
;    int k0_reg2;    /* PI parameter - proportional gain Q9 */
;    int k1_reg2;    /* PI parameter - integral time * sample time Q13 */
;    int kc_reg2;    /* PI parameter - sampling time / integral time Q13 */
;    int un_reg2;    /* Integral component of PI Q15 */
;    int en0_reg2;   /* refrence signal - feedback signal Q15 */
;    int upi_reg2;   /* actual PI output without taking into account saturation Q15 */
;                    /* i.e. if output is not saturated out_reg2 = upi_reg2 */
;    int epi_reg2;   /* out_reg2 - upi_reg2 Q15 */
;    int max_reg2;   /* PI parameter - upper cut off saturation limit of PI regulator output Q15 */
;    int min_reg2;   /* PI parameter - lower cut off saturation limit of PI regulator output Q15 */
;    int out_reg2;   /* final PI regulator output Q15 */
;
;    int (*calc)();  /* Pointer to the calculation function */
;
;  } ;
;
;================================================================================
 
     .def _pid2_calc
;================================================================================          
_pid2_calc:

__PID2_framesize .set  0001h    
;================================================================================
                                ; AR0 is the frame pointer, AR1 is the stack 
                                ; pointer
                                ; ARP = AR1
                                
    POPD  *+                    ; Save the return address from hardware
                                ; stack onto the software stack
                                ; ARP = AR1
                                
    SAR AR0, *+                 ; push AR0 (FP). ARP = AR1
    SAR AR1, *                  ; *SP = SP. ARP = AR1    
    LAR AR0, #__PID2_framesize  ; FP = size of frame. ARP = AR1
;--------------------------------------------------------------------------------                        
                SPM 0           ; set product mode to shift left zero bit    
;================================================================================
                LAR AR0, *0+, AR3
                                ; allocate frame. 
                                ; AR0 = *AR1
                                ; AR1 = AR1 + AR0
                                ; ARP = AR3
;--------------------------------------------------------------------------------                                                    
                LAR   AR3, #-3  ; AR3 = -3
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------    
                MAR   *0+       ; AR3 = AR0 - 3
                                ; ARP = AR3
                                ; AR3 -> Passed parameter 
                                ; (i.e. pointer to structure)
;--------------------------------------------------------------------------------    
                LAR   AR3, *    ; AR3 points to the first structure member 
                                ; i.e. AR3 -> fb_reg2
                                ; ARP = AR3
;--------------------------------------------------------------------------------                                             
                MAR   *+        ; AR3 points to the second structure member i.e. 
                                ; ref_reg2
                                ; ARP = AR3. AR3-> ref_reg2.
;--------------------------------------------------------------------------------                               
                LACC  *-        ; Accumulator = ref_reg2
                                ; ARP = AR3
                                ; AR3 -> fb_reg2
;--------------------------------------------------------------------------------                                  
                SUB   *         ; Accumulator = ref_reg2[Q15] - fb_reg2[Q15]
                                ; ARP = AR3. AR3 -> fb_reg2
;--------------------------------------------------------------------------------    
                ADRK  #6        ; AR3 -> en0_reg2
                                ; ARP = AR3.  
;--------------------------------------------------------------------------------                
                SACL  *-        ; en0_reg2[Q15] = ref_reg2[Q15] - fb_reg2[Q15]
                                ; AR3 -> un_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------    
                LACC  *+, 9     ; ACC[Q24] = (un_reg2[Q15] << 9)[Q24]. 
                                ; AR3 -> en0_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------    
                LT    *         ; TREG = en0_reg2[Q15]
                                ; ARP = AR3. AR3 -> en0_reg2
;--------------------------------------------------------------------------------    
                SBRK  #4        ; AR3 -> k0_reg2[Q9]
                                ; ARP = AR3
;--------------------------------------------------------------------------------    
                MPY   *         ; PREG[Q24] = k0_reg2[Q9] * en0_reg2[Q15]
                                ; ARP = AR3.
                                ; AR3 -> k0_reg2[Q9]
;--------------------------------------------------------------------------------                
                APAC            ; ACC[Q24] = ACC[Q24] + shifted PREG[Q24]. 
                                ; Here shifting is by 0 bits since SPM = 0
                                ; ACC[Q24] = k0_reg2[Q9] * en0_reg2[Q15] +
                                ; (un_reg2[Q15] << 9)[Q24]
                                ; ARP = AR3. 
                                ; AR3 -> k0_reg2[Q9]
;--------------------------------------------------------------------------------                                               
                RPT   #6
                 NORM *         ; ACC[Q31]. AR3 -> k0_reg2[Q9]
                NOP             ; ARP = AR3. 
                NOP             ; NOP is used to avoid "pipe line conflict"
;--------------------------------------------------------------------------------     
                ADRK  #5        ; AR3 -> upi_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------        
                SACH  *         ; upi_reg2[Q15] = (Higher order 16 bits of 
                                ; ACC[Q31])[Q15]
                                ; AR3 -> upi_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------                                
                LACC  *         ; ACC[Q15] = upi_reg2[Q15]
                                ; AR3 -> upi_reg2
                                ; ARP = AR3.  
;--------------------------------------------------------------------------------            
                ADRK  #3        ; AR3 -> min_reg2
                                ; ARP = AR3. 
;--------------------------------------------------------------------------------            
                ADD   *         ; ACC = upi_reg2 + min_reg2
                                ; ARP = AR3. AR3 -> min_reg2
;--------------------------------------------------------------------------------