?? encoder.c
字號(hào):
/*=====================================================================
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;
}
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -