abs_enc.c

Go to the documentation of this file.

#include <includes.h>

static volatile AE_ID ae_current_encoder; 
static AE_ENCODER ae_encoders[2]; 
static volatile AE_STATUS ae_status = AE_DONE; 
void ae_init_encoder(AE_ID encoder_id, int zero, int rpm, int cpr){
  if (encoder_id < AE_LAST){
    AE_ENCODER* encoder = &(ae_encoders[encoder_id]);
    encoder->read = 1;
    encoder->value = 0;
    encoder->zero = zero;
    encoder->delta = (int)((float)rpm * (float)cpr / (60.0 * 1000.0 * SCHED_SPEED)) + 5;
  } else {
    error_occurred(ERROR_AE_INVALID_ID);
  }
}

void ae_update(void)
{                                                       
  if (ae_encoders[AE_1].read){ //if reading from encoder 1 or both encoder 1 and 2
    ae_read(AE_1); //begin read from encoder 1
    ae_status = AE_NOT_DONE; 
  } else if (ae_encoders[AE_2].read){ //if only reading from encoder 2
    ae_read(AE_2); //begin read from encoder 2
    ae_status = AE_NOT_DONE;
  } //not reading any encoders
}

static void ae_read(AE_ID encoder_id)
{
  S0SPCR |= (1<<7); //enable interrupts
  switch (encoder_id){
    case AE_1: FIO0CLR = (1<<13); break; //set P0.13 low to read from J3
    case AE_2: FIO0SET = (1<<13); break; //set P0.13 high to read from J9
    default: error_occurred(ERROR_AE_INVALID_ID); break;
  }
  ae_current_encoder = encoder_id; //we are currently waiting for data for this encoder
  S0SPDR = 0; //send dummy data to begin transfer
}
  
int ae_get_pos(AE_ID encoder_id)
{
  if (encoder_id < AE_LAST){ //valid encoder id
    AE_ENCODER* encoder = &(ae_encoders[encoder_id]);
    return encoder->value - encoder->zero;
  } else {
    error_occurred(ERROR_AE_INVALID_ID);
    return 0;
  }
} 

void ae_wait(void){
  while (ae_status != AE_DONE) {} 
}

void ae_isr(void) __irq
{
  int int_state = S0SPSR;
  int new_data;
  AE_ENCODER* enc;
  
  S0SPINT = 1;            //clear interrupt flag in SPI register
  
  // ***** READ NEW DATA ***** //
  enc = &(ae_encoders[ae_current_encoder]);
  new_data = S0SPDR&0x1FFF; //read data from SPI
  //make sure data is valid
  if (new_data > enc->value + enc->delta){ //over bounds, limit
    new_data = enc->value + enc->delta;
    error_occurred(ERROR_AE_SPIKE); //report error
  } else if (new_data < enc->value - enc->delta){ //below bounds, limit
    new_data = enc->value - enc->delta;
    error_occurred(ERROR_AE_SPIKE); //report error
  }
  
  enc->value = new_data;
  
  // ***** START NEXT ENCODER, OR FINISHED ***** //
  if (ae_current_encoder == AE_1){ //currently reading data from encoder 1
    if (ae_encoders[AE_2].read){ //if also using encoder 2
      ae_read(AE_2); //begin read from encoder 2
    } else { //otherwise we were only reading from encoder 1
      ae_status = AE_DONE; //so we're done!
      S0SPCR &= ~(1<<7); //disable interrupts
    }
  } else if (ae_current_encoder == AE_2){ //currently reading data from encoder 2
    ae_status = AE_DONE; //always do encoder 1 then 2, so we're done!
    S0SPCR &= ~(1<<7);  //disable interrupts
  }
  
  VICVectAddr = 0;          //acknowledge interrupt, clear vector to interrupt function   
}