qec.c File Reference

A Quadrature Encoder Controller. More...

Defines
#define	QEC_MAX_BUFFER_LENGTH (64)
Functions
static void	qec_calc_velocity (volatile QEC_DATA *data, int ticks_elapsed)
	Updates the data needed to calculate velocity.
int	qec_get_abs_pos (QEC_ID id)
	Returns the absolute position of the given encoder.
float	qec_get_velocity (QEC_ID id)
	Returns the velocity, in counts/sec, of the given encoder.
void	qec_init (QEC_RESOLUTION enc_res_1, int enc1_avg_time, QEC_RESOLUTION enc_res_2, int enc2_avg_time, VOID_VOID_F function)
	Initializes the quadrature encoder module.
static void	qec_init_data (volatile QEC_DATA data, QEC_RESOLUTION resolution, int pos_buf, int *time_buf, int length)
	Initializes an encoder's data.
int	qec_is_stopped (QEC_ID id)
	Returns 1 if the motor has stopped (time since last pulse was 2 ms), 0 otherwise.
void	qec_isr (void)
	The interrupt service routine for the qec module used by Timer 0.
static void	qec_update (volatile QEC_DATA *data, int flag_chA, int flag_chB, int chA, int chB)
	Updates the given encoder with data from the timer.
void	qec_update_velocity (void)
	MUST call this every schedule tick.
Variables
static volatile QEC_DATA	qec_data1
	The encoder data for the first encoder - J3.
static volatile QEC_DATA	qec_data2
	The encoder data for the second encoder - J9.
static unsigned int	qec_match_ticks = 0
	Number of times the overflow (match value) has been reached and the timer reset.
static VOID_VOID_F	qec_overflow_funct = voidvoid
	The function to call when a timer match and reset occurs.
static int	qec_pos_buf1 [QEC_MAX_BUFFER_LENGTH]
static int	qec_pos_buf2 [QEC_MAX_BUFFER_LENGTH]
static unsigned int	qec_prev_ticks = 0
	Previous value of the counter.
static float	qec_ticks_per_sec = 60000000.0f
static int	qec_time_buf1 [QEC_MAX_BUFFER_LENGTH]
static int	qec_time_buf2 [QEC_MAX_BUFFER_LENGTH]

Detailed Description

A Quadrature Encoder Controller.

Each quadrature encoder on a motor is represented by a QEC_DATA struct. The positive direction is defined as counter-clockwise when looking at the motor from the front.

A value of QEC_NULL in initialization will ignore both channels A and B, but the interrupts will continue to occur unless they are disabled in Hardware Setup.

A value of QEC_2X will ignore channel B interrupts, but similarly, interrupts for that channel will need to be disabled in Hardware Setup for that channel to take full advantage of the time savings.

A value of QEC_4X will use values from both channels, and the interrupts will need to be setup correctly.

This module uses Timer 0, but can be setup to run the scheduler if asched_tick is passed to the overflow function.

Example Hardware and Register Setup of pins and timer 0:

  // *******************************************************************************
  // QEC Setup
  // *******************************************************************************
  //Enable Capture Interrupt For Falling and Rising Edges:
  PINSEL1 &= ~(3<<12);
  PINSEL1 &= ~(3<<0); //Clear bits 1:0 (P0.16)
  PINSEL1 &= ~(3<<26); //Clear bits 27:26 (P0.29)
  PINSEL1 &= ~(3<<22);
  PINSEL1 |= (2<<12);    // set pin 0.22 to capture 0.0  
  PINSEL1 |= (2<<22);    // set pin 0.27 to capture 0.1
  PINSEL1 |= (3<<0); //Set P0.16 to Timer0 Capture 0.2
  PINSEL1 |= (2<<26); //Set P0.29 to Timer0 Capture 0.3
  //Set up the Timer Counter 0 Interrupt
  //Used to recod length of time between encoder pulses.
  T0IR  = 0xFF;                           //clear interrupts in Timer0
  //Enable Capture registers
  //Encoder 1 - J3 (Can comment out as needed for desired resolution)
  T0CCR |= (1<<0)|(1<<1)|(1<<2); //Cap0.0 on rising and falling edges
  T0CCR |= (1<<3)|(1<<4)|(1<<5); //Cap0.1 on rising and falling edges
  //Encoder 2 - J9 (Can comment out as needed for desired resolution)
  T0CCR |= (1<<6)|(1<<7)|(1<<8);//|(1<<9)|(1<<10)|(1<<11); //Cap0.2 on rising and falling edges
  T0CCR |= (1<<9)|(1<<10)|(1<<11); //Cap0.3 on rising and falling edges

Example interrupt setup (currently using FIQ):

  add_fiq(VIC_TIMER0, qec_isr);

Author:: Nicolas Williamson; Thomas Craig

Date:: June 2009

Definition in file qec.c.

Function Documentation

static void qec_calc_velocity	(	volatile QEC_DATA *	data,
		int	ticks_elapsed
	)			`[static]`

Updates the data needed to calculate velocity.

Parameters:

	data	The encoder to update the data of.
	ticks_elapsed	The number of ticks since the last call to update_velocity.

Definition at line 180 of file qec.c.

References QEC_DATA::current_pos, QEC_DATA::delta_pos, QEC_DATA::delta_time, QEC_DATA::index, QEC_DATA::length, QEC_DATA::pos_buf, QEC_DATA::prev_pos, and QEC_DATA::time_buf.

Referenced by qec_update_velocity().

00180                                                                          {
00181   int new_pos = data->current_pos - data->prev_pos;
00182   int old_pos = data->pos_buf[data->index];
00183   int new_time = ticks_elapsed;
00184   int old_time = data->time_buf[data->index];
00185 
00186   data->pos_buf[data->index] = new_pos;
00187   data->time_buf[data->index] = new_time;
00188   data->index = (data->index + 1) % data->length;
00189   data->prev_pos = data->current_pos;
00190   data->delta_pos = data->delta_pos + new_pos - old_pos;
00191   data->delta_time = data->delta_time + new_time - old_time; 
00192 }

int qec_get_abs_pos ( QEC_ID id )

Returns the absolute position of the given encoder.

Parameters:

id

The id of the encoder, either QEC_1 (J3) or QEC_2 (J9).

Returns:: The absolute position in encoder counts.

Definition at line 150 of file qec.c.

References QEC_DATA::current_pos, and error_occurred().

00151 {
00152   switch (id) {
00153     case QEC_1: return qec_data1.current_pos;
00154     case QEC_2: return qec_data2.current_pos;
00155     default: error_occurred(ERROR_QEC_BAD_ID); return 0;
00156   }
00157 }

float qec_get_velocity ( QEC_ID id )

Returns the velocity, in counts/sec, of the given encoder.

Parameters:

id

The id of the encoder to read from, either QEC_1 (J3) or QEC_2 (J9).

Returns:: The velocity of the encoder in counts/sec.

Definition at line 199 of file qec.c.

References QEC_DATA::delta_pos, QEC_DATA::delta_time, error_occurred(), and QEC_DATA::velocity.

Referenced by qec_is_stopped().

00200 {
00201   volatile QEC_DATA* data;
00202   switch (id) {
00203     case QEC_1: data = &qec_data1; break;
00204     case QEC_2: data = &qec_data2; break;
00205     default: error_occurred(ERROR_QEC_BAD_ID); return 0.0;
00206   }
00207   data->velocity = (float)((data->delta_pos * qec_ticks_per_sec) / (float)data->delta_time);
00208   return data->velocity;
00209 }

void qec_init	(	QEC_RESOLUTION	enc_res_1,
		int	enc1_avg_time,
		QEC_RESOLUTION	enc_res_2,
		int	enc2_avg_time,
		VOID_VOID_F	function
	)

Initializes the quadrature encoder module.

Encoders are enabled with the given resolution, and the overflow function will be called whenever there is a match and reset on match 0 of the timer.

Parameters:

	enc_res_1	The desired resolution for encoder 1, either NULL, 2X, or 4X.
	enc1_avg_time	The time in ms over which the velocity is averaged for encoder 1.
	enc_res_2	The desired resolution for encoder 2, either NULL, 2X, or 4X.
	enc2_avg_time	The time in ms over which the velocity is averaged for encoder 2.
	function	This function will be called when a timer reset occurs.

Definition at line 80 of file qec.c.

References qec_init_data(), qec_match_ticks, and qec_overflow_funct.

00081 {
00082   if (enc1_avg_time * SCHED_SPEED < QEC_MAX_BUFFER_LENGTH){
00083     qec_init_data(&qec_data1, enc_res_1, qec_pos_buf1, qec_time_buf1, enc1_avg_time* SCHED_SPEED);
00084   }
00085   if (enc2_avg_time * SCHED_SPEED < QEC_MAX_BUFFER_LENGTH){
00086     qec_init_data(&qec_data2, enc_res_2, qec_pos_buf2, qec_time_buf2, enc2_avg_time* SCHED_SPEED);
00087   }
00088   qec_overflow_funct = function;
00089   qec_match_ticks = T0MR0;
00090 }

static void qec_init_data	(	volatile QEC_DATA *	data,
		QEC_RESOLUTION	resolution,
		int *	pos_buf,
		int *	time_buf,
		int	length
	)			`[static]`

Initializes an encoder's data.

Parameters:

	data	A pointer to the encoder data struct.
	time_buf	The time buffer.
	pos_buf	The position buffer.
	length	The length of both buffers.
	resolution	The resolution desired for the encoder.

Definition at line 100 of file qec.c.

References QEC_DATA::current_pos, QEC_DATA::index, QEC_DATA::length, QEC_DATA::pos_buf, QEC_DATA::resolution, and QEC_DATA::time_buf.

Referenced by qec_init().

00101 {
00102   data->current_pos = 0;
00103   data->resolution = resolution;
00104   data->pos_buf = pos_buf;
00105   data->time_buf = time_buf;
00106   data->length = length;
00107   data->index = 0;
00108 }

int qec_is_stopped ( QEC_ID id )

Returns 1 if the motor has stopped (time since last pulse was 2 ms), 0 otherwise.

Parameters:

id

The encoder to check.

Returns:: 1: motor stopped, 0: motor moving

Definition at line 217 of file qec.c.

References qec_get_velocity().

00217                              {
00218   return (qec_get_velocity(id) < 0.2); //0.38 from old version
00219 }

void qec_isr ( void )

The interrupt service routine for the qec module used by Timer 0.

Called whenever there is an edge in channel A or B, or timer 0 matched and was reset.

Definition at line 115 of file qec.c.

References qec_overflow_funct, qec_update(), and QEC_DATA::resolution.

00116 {
00117   unsigned int count = T0TC;
00118   unsigned int flags = T0IR; //save the interrupt flags
00119   unsigned int pins = FIO0PIN;  //save the pin state
00120   int flag_overflow = flags & (1<<0);
00121   int flag_cap0 = flags & (1<<4); 
00122   int flag_cap1 = flags & (1<<5);
00123   int flag_cap2 = flags & (1<<6);
00124   int flag_cap3 = flags & (1<<7);
00125   int chA1 = pins & (1<<22); //encoder 1
00126   int chB1 = pins & (1<<27); //""
00127   int chA2 = pins & (1<<16); //encoder 2
00128   int chB2 = pins & (1<<29); //""
00129   
00130   //Clear interrupt register flags
00131   T0IR = flag_overflow | flag_cap0 | flag_cap1 | flag_cap2 | flag_cap3;
00132   
00133   //Only update information for an encoder struct that exists
00134   if (qec_data1.resolution != QEC_NULL){
00135     qec_update(&qec_data1, flag_cap0, flag_cap1, chA1, chB1);
00136   }
00137   if (qec_data2.resolution != QEC_NULL){
00138     qec_update(&qec_data2, flag_cap2, flag_cap3, chA2, chB2);
00139   }   
00140   if (flag_overflow){
00141     qec_overflow_funct();
00142   }
00143 }

static void qec_update	(	volatile QEC_DATA *	data,
		int	flag_chA,
		int	flag_chB,
		int	chA,
		int	chB
	)			`[static]`

Updates the given encoder with data from the timer.

Called from qec_isr for both encoders, if applicable.

Parameters:

	data	A pointer to the encoder's data struct.
	flag_chA	Whether a capture interrupt occurred on channel A.
	flag_chB	Whether a capture interrupt occurred on channel B.
	chA	The state (1 = hi, 0 = lo) of channel A.
	chB	The state of channel B.

Definition at line 229 of file qec.c.

References QEC_DATA::current_pos, error_occurred(), QEC_DATA::prev_chA, QEC_DATA::prev_chB, and QEC_DATA::resolution.

Referenced by qec_isr().

00231 00232 00233 00234 00235 00236   } 00237 00238 00239 00240 00241   } 00242 00243 00244 00245 00246 00247 00248 00249 00250 00251 00252 00253         } 00254 00255 00256 00257 00258 00259         } 00260       } 00261 00262 00263     } 00264 00265 00266 00267 00268 00269 00270 00271 00272 00273         } 00274 00275 00276 00277 00278 00279         } 00280       } 00281 00282 00283     } 00284   } 00285 00286 }

class="fragment">00230 { int delta_pos; //if resolution only 2x, ignore channel B if (data->resolution == QEC_2X){ flag_chB = 0; //both channels can't interrupt this fast; that would suck if (flag_chA && flag_chB){ error_occurred(ERROR_QEC_BOTH_CHS); // If captures occurred if (flag_chA){ if (data->prev_chA == chA){ //we are back even though we had an interrupt, MISSED COUNT! error_occurred(ERROR_QEC_MISSED_A); //missed a count on channel A } else { if (chA){ //A high if (chB) { //B high (B & A & fA) delta_pos = -1; } else { //B low (!B & A & fA) delta_pos = 1; } else { //A low if (chB) { //B high (B & !A & fA) delta_pos = 1; } else { //B low (!B & !A & fA) delta_pos = -1; data->current_pos += delta_pos; data->prev_chA = chA; } else if (flag_chB){ if (data->prev_chB == chB){ //missed count error_occurred(ERROR_QEC_MISSED_B); } else { if (chA){ //A high if (chB) { //B high delta_pos = 1; }else { //B low delta_pos = -1; } else { //A low if (chB) { //B high delta_pos = -1; } else { //B low delta_pos = 1; data->current_pos += delta_pos; data->prev_chB = chB;

Variable Documentation

volatile QEC_DATA qec_data1 [static]

The encoder data for the first encoder - J3.

Definition at line 64 of file qec.c.

volatile QEC_DATA qec_data2 [static]

The encoder data for the second encoder - J9.

Definition at line 65 of file qec.c.

unsigned int qec_match_ticks = 0 [static]

Number of times the overflow (match value) has been reached and the timer reset.

Definition at line 67 of file qec.c.

Referenced by qec_init(), and qec_update_velocity().

VOID_VOID_F qec_overflow_funct = voidvoid [static]

The function to call when a timer match and reset occurs.

Definition at line 66 of file qec.c.

Referenced by qec_init(), and qec_isr().

unsigned int qec_prev_ticks = 0 [static]

Previous value of the counter.

Definition at line 68 of file qec.c.

Referenced by qec_update_velocity().

qec.c File Reference

Defines

Functions

Variables

Detailed Description

Function Documentation

Variable Documentation