qec.c

Go to the documentation of this file.
00001 
00056 #include <includes.h>
00057 
00058 // Global Variables
00059 #define QEC_MAX_BUFFER_LENGTH (64)
00060 static int qec_pos_buf1[QEC_MAX_BUFFER_LENGTH];
00061 static int qec_pos_buf2[QEC_MAX_BUFFER_LENGTH];
00062 static int qec_time_buf1[QEC_MAX_BUFFER_LENGTH];
00063 static int qec_time_buf2[QEC_MAX_BUFFER_LENGTH];
00064 static volatile QEC_DATA qec_data1; 
00065 static volatile QEC_DATA qec_data2; 
00066 static VOID_VOID_F qec_overflow_funct = voidvoid; 
00067 static unsigned int qec_match_ticks = 0; 
00068 static unsigned int qec_prev_ticks = 0; 
00069 static float qec_ticks_per_sec = 60000000.0f; //(float)1.0/60000000.0; sec/count
00070 
00080 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)
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 }
00091 
00100 static void qec_init_data(volatile QEC_DATA* data, QEC_RESOLUTION resolution, int* pos_buf, int* time_buf, int length)
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 }
00109 
00110 
00115 void qec_isr(void) //__irq
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 }
00144 
00150 int qec_get_abs_pos(QEC_ID id)
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 }
00158 
00162 void qec_update_velocity(void){
00163   unsigned int current_ticks = T0TC;
00164   int ticks_elapsed = qec_match_ticks + (current_ticks - qec_prev_ticks);
00165   qec_prev_ticks = current_ticks;
00166   
00167   if (qec_data1.resolution != QEC_NULL){
00168     qec_calc_velocity(&qec_data1, ticks_elapsed);
00169   } 
00170   if (qec_data2.resolution != QEC_NULL){
00171     qec_calc_velocity(&qec_data2, ticks_elapsed);
00172   }
00173 }
00174 
00180 static void qec_calc_velocity(volatile QEC_DATA* data, int ticks_elapsed){
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 }
00193       
00199 float qec_get_velocity(QEC_ID id)
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 }
00210 
00217 int qec_is_stopped(QEC_ID id){
00218   return (qec_get_velocity(id) < 0.2); //0.38 from old version
00219 }
00220 
00229 static void qec_update(volatile QEC_DATA* data, int flag_chA, int flag_chB, int chA, int chB)
00230 {
00231   int delta_pos;
00232 
00233   //if resolution only 2x, ignore channel B
00234   if (data->resolution == QEC_2X){
00235     flag_chB = 0;
00236   }
00237   
00238   //both channels can't interrupt this fast; that would suck
00239   if (flag_chA && flag_chB){
00240     error_occurred(ERROR_QEC_BOTH_CHS);
00241   }
00242 
00243   // If captures occurred
00244   if (flag_chA){
00245     if (data->prev_chA == chA){ //we are back even though we had an interrupt, MISSED COUNT!
00246       error_occurred(ERROR_QEC_MISSED_A); //missed a count on channel A
00247     } else {
00248       if (chA){ //A high
00249         if (chB) { //B high (B & A & fA)
00250           delta_pos = -1; 
00251         } else { //B low (!B & A & fA)
00252           delta_pos = 1;
00253         }
00254       } else { //A low
00255         if (chB) { //B high (B & !A & fA)
00256           delta_pos = 1;
00257         } else { //B low (!B & !A & fA)
00258           delta_pos = -1;
00259         }   
00260       } 
00261       data->current_pos += delta_pos;
00262       data->prev_chA = chA;
00263     }
00264   } else if (flag_chB){
00265     if (data->prev_chB == chB){ //missed count
00266       error_occurred(ERROR_QEC_MISSED_B);
00267     } else {
00268       if (chA){ //A high
00269         if (chB) { //B high
00270           delta_pos = 1;  
00271         }else { //B low
00272           delta_pos = -1;
00273         }
00274       } else { //A low
00275         if (chB) { //B high
00276           delta_pos = -1;
00277         } else { //B low
00278           delta_pos = 1;
00279         }   
00280       } 
00281       data->current_pos += delta_pos;
00282       data->prev_chB = chB;
00283     }
00284   }
00285   
00286 } 
00287 
00288 
00289 
00290 
00291 
00292 
00293 
00294 
00295 
00296 
00297 
Generated on Tue Jun 29 16:36:14 2010 by  doxygen 1.6.3