encoder.c

Texas-Instrument C2000 Series DSP example programs

/*=====================================================================
  Name:           ENCODER.C
  Project:        ENCODER
  Originator:     Martin Staebler
  Description:    Provides functions for handling incremental
                  encoder with analog sin/cos output signal
                  to achieve higher position resolution
 =======================================================================
 
       Function List:   void   Encoder_Init(unsigned rollover)
                        void   Encoder_ZeroPosition(void)
                        void   Encoder_MatchIncrPhase(void)
                        void   Encoder_CalcPhase(int sin,int ncos) 
                        unsigned Encoder_SamplePosition(void)
                        void   Encoder_CalcPosition(unsigned qep_incr_sample)

        Status:         OK

        Target:         TMS320C240

        History:        (Date, Revision, Who, What)
        ------------------------------------------------------------
        02/11/97        1.0     STAE    Preliminary
 ======================================================================*/
				 

/*--------------*/
/* Header Files */
/*--------------*/
#include <stdlib.h>             
#include <math.h>       
#include <q15_div.h>            /* q15.lib */
#include <q15_atan.h>           /* q15.lib */
#include "c240.h"               /* TMS320C240 memory mapped registers */
#include "adc.h"                /* ADC macros */
#include "evm_qep.h"            /* QEP unit */
#include "monitor.h"            /* test program & monitor */
#include "encoder.h"            /* encoder definition */



/*----------------------*/
/* Variable Declaration */
/*----------------------*/
volatile unsigned  encoder_position[2];    



/*======================================================================*/
/* void Encoder_Init(unsigned rollover)                                 */
/*======================================================================*/
/* Function:      Incremental encoder initialization			*/
/* 									*/
/* Arguments:     Incremental counts/revolution = 4-times line count    */
/* 									*/
/* Return value:  None							*/	
/*======================================================================*/
void Encoder_Init(unsigned rollover)
{
    /*---------------------*/
    /* Initialize F240 ADC */
    /*---------------------*/
    ADCTRL1  = 0x1800;           /* enable both ADC's */
    ADCTRL1 |= 0x0100;           /* clear interrupt flag */       
    ADCTRL2  = 0x0003;           /* ADC_CLOCK = SYSCLK/10 = 1MHz */

    /*------------------------------*/
    /* Initialize F240 QEP Circuit  */
    /*------------------------------*/
    QEP_Init(0x0,rollover);      /* QEP counts Timer 2 */

    /*------------------------------------------------*/
    /* Get zero incremental position (via index) and  */
    /* synchronize incremental count and phase        */
    /*------------------------------------------------*/
    Encoder_MSG();              /* turn encoder into zero position */
    Encoder_ZeroPosition();     /* detect zero position adjust counter */
    Encoder_MatchIncrPhase();   /* match counter (quadrant) and phase */

} /* end Encoder_Init */



/*======================================================================*/
/* void Encoder_ZeroPosition(void)                                     */
/*======================================================================*/
/* Function:  Modify/adjust qep_diff, so that the incremental count of  */
/*            the encoder (using function QEP_GetIncr() matches with    */
/*            phase derived from the analog sin/-cos signal, as shown   */
/*            below.                                                    */
/*======================================================================*/
void Encoder_ZeroPosition(void)
{
   /* Configure IOPC6/CAP3 to detect level */
   OCRB &= 0xFFBF;                      /* clear bit 6 */
   PCDATDIR &= 0xBFFF;                  /* clear bit 14 */
   while ( !(PCDATDIR & 0x0040));       /* poll bit 6 */
   T2CNT = 0x0;
}

/*======================================================================*/
/* void Encoder_MatchIncrPhase(void)                                    */
/*======================================================================*/
/* Function:  Modify/adjust qep_diff, so that the incremental count of  */
/*            the encoder (using function QEP_GetIncr() matches with    */
/*            phase derived from the analog sin/-cos signal, as shown   */
/*            below.                                                    */
/*======================================================================*/
void Encoder_MatchIncrPhase(void)
{
    /*-----------------*/
    /* local variables */
    /*-----------------*/
    char  c;
    volatile int   	buffer[2];
    volatile unsigned 	ubuffer[2];

    /*----------------------------------------------------------*/
    /* Synchronize incremental count and encoder phase  	*/
    /*----------------------------------------------------------*/
    /* Assumption: ADCIN5  <--> QEP1/sin(x)                  	*/
    /* 		   ADCIN13 <--> QEP2/negcos(x)                 	*/
    /* 								*/
    /* (T2CNT % QEP_ENCODER) % 4 + qep_diff =  Quadrant 0,1,2,3 */
    /*----------------------------------------------------------*/
    do
    {
       /*------------------------*/
       /* aquire encoder signals */
       /*------------------------*/
       ubuffer[1] = Encoder_SamplePosition();

       ADC_READ2(buffer[0],buffer[1]);

       /* Correct U0 and U90 input signals offset */
       buffer[0] = buffer[0] - ENC_U0_OFFSET;  
       buffer[1] = buffer[1] - ENC_U90_OFFSET;

       /*----------------------------------------------*/
       /* check, if nearly in the middle of a quadrant */ 
       /*----------------------------------------------*/
    }  while ( abs(abs(buffer[0])-abs(buffer[1])) > 0x4000);

    /*----------------------------------------*/
    /* init qep_diff for phase/count matching */
    /*----------------------------------------*/
    ubuffer[0] = Encoder_CalcPhase(buffer[0],buffer[1]);   
    ubuffer[0] = (ubuffer[0] >> 14) & 0x0003;   /* extract quadrant */
    ubuffer[1] = ubuffer[1] & 0x0003;
    
    qep_diff = qep_diff + ((int) ubuffer[1] - (int) ubuffer[0]); 
}    


/*======================================================================*/
/* unsigned Encoder_SamplePosition(void);                               */
/*======================================================================*/
/* Function:      Sample encoder signals SIMULTANEOUSLY                 */
/*                SIN, -COS  --> Channel 5, 13 (hardcoded)              */
/*                INCREMENTS --> Timer 2 counter (T2CNT)                */
/* 									*/
/* Arguments:     Buffer for increments sample                          */ 
/* 									*/
/* Return value:  Incremental count                                     */ 
/*======================================================================*/
asm("ADCTRL1    .set    7032h                                   ");
asm("T2CNT      .set    7405h                                   ");
asm("           .ref    _qep_diff                               ");
asm("           .ref    _QEP_GetIncr                            ");
asm("           .def    _Encoder_SamplePosition                 ");
asm("_Encoder_SamplePosition:                                   ");
asm("           ldp     #ADCTRL1/128                            ");
asm("           lacl    ADCTRL1                                 ");
asm("           and     #0FF81h         ;clear channels         ");
asm("           or      #005Bh          ;select channel 5 and 13");
asm("           sacl    ADCTRL1         ;(1) start ADC's        ");

#ifdef QEP_POWER2
asm("           ldp     #_qep_diff                              ");
asm("           lacl    #0                                      ");
asm("           sub     _qep_diff                               ");
asm("           ldp     #(T2CNT/128)                            ");
asm("           nop                                             ");
asm("     ;capture Timer 2 300ns after ADC start (1)            ");
asm("     ;-----------------------------------------            ");
asm("           add     T2CNT                                   ");
asm("           and     #(QEP_ROLLOVER-1)                       ");
asm("           ret                                             ");

#else
asm("     ;capture Timer 2 300ns after ADC start (1)                  ");
asm("     ;-----------------------------------------                  ");
asm("           call    _QEP_GetIncr   ;200ns (call) + 100ns in subroutine ");
asm("           ret                                                   ");

#endif



/*======================================================================*/
/* void Encoder_CalcPosition(unsigned qep_incr_sample);                 */
/*======================================================================*/
/* Function:      Calc encoder position                                 */
/*                               increments (Timer 2)                   */
/*                                                                      */
/* Arguments:     increments sample                                     */                                                  
/* 									*/
/* Global var's:  unsigned qep_position[2]                              */                                                  
/* 									*/
/* Return value:  None							*/	
/*======================================================================*/
void Encoder_CalcPosition(unsigned qep_incr_sample)
{
    /*--------------*/
    /* locals var's */
    /*--------------*/
    volatile int sin_sample;
    volatile int ncos_sample;
    volatile unsigned incr;
    volatile unsigned buffer;

    incr = qep_incr_sample;

    /*---------------------------------------------*/
    /* read converted sin and -cos encoder signals */
    /*---------------------------------------------*/
    ADC_READ2(sin_sample,ncos_sample);

    /* Correct U0 (sin_sample and U90 (ncos_sample) offset */
    sin_sample  = sin_sample - ENC_U0_OFFSET; 
    ncos_sample = ncos_sample - ENC_U90_OFFSET;

    /*-------------------*/
    /* phase calculation */
    /*-------------------*/
    encoder_position[0] = Encoder_CalcPhase(sin_sample, ncos_sample);   
   
    /*----------------------------------------------------------*/
    /* correct incremental steps according to phase information */
    /*----------------------------------------------------------*/
    buffer = ((encoder_position[0] >> 14) & 0x0003);
    switch (buffer)
    {
       case 0:  if ((incr & 0x0003) == 3)
                   incr = (incr + 1) & (qep_rollover-1);
                break;  

       case 3:  if ((incr & 0x0003) == 0)
                   incr = (incr - 1) & (qep_rollover-1);
                break;  
    } /* switch */

    /* remove (redundant) quadrant information (last two bits) */   
    encoder_position[1] = (incr >> 2);
}



/*======================================================================*/
/* int Encoder_CalcPhase(int qep_sin,int qep_negcos);                   */ 
/*======================================================================*/
/* Function:      Incremental encoder initialization			*/
/* 									*/
/* Arguments:     None							*/
/* 									*/
/* Return value:  None							*/	
/*======================================================================*/
int  Encoder_CalcPhase(int qep_sin, int qep_negcos)  
{

    int  phase;
    int  buffer;
   /*------------------------------------------*/
   /* general calculation, within 1st quadrant */
   /*------------------------------------------*/
   if (abs(qep_sin) == abs(qep_negcos))
      phase = (PI/4);               
   else if (abs(qep_sin) < abs(qep_negcos))
   {
      buffer = q15_div(abs(qep_sin),abs(qep_negcos)); 
      /* phase  = q15_atan(buffer);  */ 
      phase  = q15p_atan(buffer);   
   } 
   else
   {
      buffer = q15_div(abs(qep_negcos),abs(qep_sin)); 
      /* phase  = (PI/2) - q15_atan(buffer); */  
      phase  = (PI/2) - q15p_atan(buffer);   
   }
   
   /*------------------------------*/
   /* get 2nd, 3rd to 4th quadrant */
   /*------------------------------*/
   if (qep_sin >= 0)   
   {   
      if (qep_negcos > 0)
         phase = PI - phase;    /* 2nd quadrant */ 
   } /* end if */
   
   else 
   {  
      if ( qep_negcos > 0 )
         phase = PI + phase;    /* 3rd quadrant */ 
      else
         phase = -phase;        /* 4th quadrant */ 
   } /* end else */
   
   return  phase; 
}