qec.c

Go to the documentation of this file.

#include <includes.h>

// Global Variables
#define QEC_MAX_BUFFER_LENGTH (64)
static int qec_pos_buf1[QEC_MAX_BUFFER_LENGTH];
static int qec_pos_buf2[QEC_MAX_BUFFER_LENGTH];
static int qec_time_buf1[QEC_MAX_BUFFER_LENGTH];
static int qec_time_buf2[QEC_MAX_BUFFER_LENGTH];
static volatile QEC_DATA qec_data1; 
static volatile QEC_DATA qec_data2; 
static VOID_VOID_F qec_overflow_funct = voidvoid; 
static unsigned int qec_match_ticks = 0; 
static unsigned int qec_prev_ticks = 0; 
static float qec_ticks_per_sec = 60000000.0f; //(float)1.0/60000000.0; sec/count

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)
{
  if (enc1_avg_time * SCHED_SPEED < QEC_MAX_BUFFER_LENGTH){
    qec_init_data(&qec_data1, enc_res_1, qec_pos_buf1, qec_time_buf1, enc1_avg_time* SCHED_SPEED);
  }
  if (enc2_avg_time * SCHED_SPEED < QEC_MAX_BUFFER_LENGTH){
    qec_init_data(&qec_data2, enc_res_2, qec_pos_buf2, qec_time_buf2, enc2_avg_time* SCHED_SPEED);
  }
  qec_overflow_funct = function;
  qec_match_ticks = T0MR0;
}

static void qec_init_data(volatile QEC_DATA* data, QEC_RESOLUTION resolution, int* pos_buf, int* time_buf, int length)
{
  data->current_pos = 0;
  data->resolution = resolution;
  data->pos_buf = pos_buf;
  data->time_buf = time_buf;
  data->length = length;
  data->index = 0;
}


void qec_isr(void) //__irq
{
  unsigned int count = T0TC;
  unsigned int flags = T0IR; //save the interrupt flags
  unsigned int pins = FIO0PIN;  //save the pin state
  int flag_overflow = flags & (1<<0);
  int flag_cap0 = flags & (1<<4); 
  int flag_cap1 = flags & (1<<5);
  int flag_cap2 = flags & (1<<6);
  int flag_cap3 = flags & (1<<7);
  int chA1 = pins & (1<<22); //encoder 1
  int chB1 = pins & (1<<27); //""
  int chA2 = pins & (1<<16); //encoder 2
  int chB2 = pins & (1<<29); //""
  
  //Clear interrupt register flags
  T0IR = flag_overflow | flag_cap0 | flag_cap1 | flag_cap2 | flag_cap3;
  
  //Only update information for an encoder struct that exists
  if (qec_data1.resolution != QEC_NULL){
    qec_update(&qec_data1, flag_cap0, flag_cap1, chA1, chB1);
  }
  if (qec_data2.resolution != QEC_NULL){
    qec_update(&qec_data2, flag_cap2, flag_cap3, chA2, chB2);
  }   
  if (flag_overflow){
    qec_overflow_funct();
  }
}

int qec_get_abs_pos(QEC_ID id)
{
  switch (id) {
    case QEC_1: return qec_data1.current_pos;
    case QEC_2: return qec_data2.current_pos;
    default: error_occurred(ERROR_QEC_BAD_ID); return 0;
  }
}

void qec_update_velocity(void){
  unsigned int current_ticks = T0TC;
  int ticks_elapsed = qec_match_ticks + (current_ticks - qec_prev_ticks);
  qec_prev_ticks = current_ticks;
  
  if (qec_data1.resolution != QEC_NULL){
    qec_calc_velocity(&qec_data1, ticks_elapsed);
  } 
  if (qec_data2.resolution != QEC_NULL){
    qec_calc_velocity(&qec_data2, ticks_elapsed);
  }
}

static void qec_calc_velocity(volatile QEC_DATA* data, int ticks_elapsed){
  int new_pos = data->current_pos - data->prev_pos;
  int old_pos = data->pos_buf[data->index];
  int new_time = ticks_elapsed;
  int old_time = data->time_buf[data->index];

  data->pos_buf[data->index] = new_pos;
  data->time_buf[data->index] = new_time;
  data->index = (data->index + 1) % data->length;
  data->prev_pos = data->current_pos;
  data->delta_pos = data->delta_pos + new_pos - old_pos;
  data->delta_time = data->delta_time + new_time - old_time; 
}
      
float qec_get_velocity(QEC_ID id)
{
  volatile QEC_DATA* data;
  switch (id) {
    case QEC_1: data = &qec_data1; break;
    case QEC_2: data = &qec_data2; break;
    default: error_occurred(ERROR_QEC_BAD_ID); return 0.0;
  }
  data->velocity = (float)((data->delta_pos * qec_ticks_per_sec) / (float)data->delta_time);
  return data->velocity;
}

int qec_is_stopped(QEC_ID id){
  return (qec_get_velocity(id) < 0.2); //0.38 from old version
}

static void qec_update(volatile QEC_DATA* data, int flag_chA, int flag_chB, int chA, int chB)
{
  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;
    }
  }
  
}