Colony

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file encoders.c

 * @brief Encoders

 *

 * Implementation of functions for encoders.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#include <avr/io.h>

#include "dragonfly_defs.h"

#include "encoders.h"

unsigned char encoders_initd=0;

int encoder_recv(char data);

int encoders_init(void){

  int i;

  if(encoders_initd)

    return ERROR_INIT_ALREADY_INITD;

  data_ready=0;

  if(spi_init(encoder_recv, encoder_recv_complete))

    return -1; //if spi was inited for something else, bail out

  encoder_buf_index = 0;

  left_data_buf = 0;

  right_data_buf= 0;

  left_data = -1;

  right_data = -1;

  //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);

  left_data_idx = 0;

  right_data_idx = 0;

  for(i = 0; i < BUFFER_SIZE; i++) {

    left_data_array[i] = 0;

  }

  for(i = 0; i < BUFFER_SIZE; i++) {

    right_data_array[i] = 0;

  }

  spi_transfer(5);

  encoders_initd=1;

  return 0;

 *         -2 means the library was not properly initialized

  if(!encoders_initd)

    return -2;

  if(encoder==LEFT) {

    return left_data;

  }

  else if(encoder==RIGHT){

    return right_data;

  }

  else{

    return -1;

  }

 * @return The distance covered by the specified encoder, -2 if the library is not initialized

  if(!encoders_initd)

    return -2;

  if(encoder==LEFT)

    return left_dx;

  else if(encoder==RIGHT)

    return right_dx;

  else

    return -1;

int encoder_rst_dx(char encoder) {

  if(!encoders_initd)

    return ERROR_LIBRARY_NOT_INITD;

  if(encoder==LEFT)

    left_dx = 0;

  else if(encoder==RIGHT)

    right_dx = 0;

  return 0;

* @return 0 if init succesfull, an error code otherwise

int encoder_rst_tc(void) {

  if(!encoders_initd)

    return ERROR_LIBRARY_NOT_INITD;

  timecount = 0;

  return 0;

*

* @return 0 if init succesfull, an error code otherwise

int encoder_wait(int n){

  if(!encoders_initd)

    return ERROR_LIBRARY_NOT_INITD;

  while(data_ready<n);

  data_ready=0;

  return 0;

/**

 * @brief This functions recieves data from the encoders directly

 *

 * @note Full reads occur every 40 microseconds. This function should be called every 8 microseconds.

 *

 * @return 0 if init succesfull, an error code otherwise

 */

int encoder_recv(char data){

  short int dx;

  if(!encoders_initd)

    return ERROR_LIBRARY_NOT_INITD;

  //Parse the encoder data, comes in over 5 bytes 16 bits per encoder,

  // second is offset by 1 bit.

  switch(encoder_buf_index){	

  case 0: 

    right_data_buf |= ((short)data)<<8 & 0xff00;

    break;

  case 1:

    right_data_buf |= ((short)data) & 0xff;

    break;

  case 2:

    left_data_buf |= (((short)data) << 9) & (0x7F << 9);

    break;

  case 3:

    left_data_buf |= (((short)data) << 1) & (0xFF<<1);

    break;

  case 4: left_data_buf |= (((short)data)>>7) & 0x1;

  }	

  encoder_buf_index = (encoder_buf_index + 1) % 5;

  if(encoder_buf_index==0) {

    /*Error handling for the left encoder*/ 

    if(!(left_data_buf & OCF)) 

      left_data = ENCODER_DATA_NOT_READY; 

    if(left_data_buf & (COF | LIN))  

      left_data = ENCODER_MISALIGNED;

    else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) 

      left_data = ENCODER_MAGNET_FAILURE;

    else left_data = (left_data_buf>>5) & 1023;

    /*Error handling for the right encoder*/ 

    if(!(right_data_buf & OCF))

      right_data = ENCODER_DATA_NOT_READY;	

    if(right_data_buf & (COF | LIN)) 

      right_data = ENCODER_MISALIGNED;

    else if ((right_data_buf & MagINCn)  && (right_data_buf & MagDECn)) 

      right_data = ENCODER_MAGNET_FAILURE;

    else right_data = (right_data_buf>>5) & 1023;

    left_data_buf = 0;

    right_data_buf = 0;

    /*Note: Above 1023 is invalid data*/	

    if(!(left_data > 1023)) {

      //Reverse the left wheel since encoders are necessarily mounted backwards.

      left_data = 1023 - left_data;

      left_data_array_put(left_data);

      //Adjust left accumulator

      dx = left_data - left_data_array_prev();

      if(left_data_array_prev()==0)  dx=0;

      if(dx > 512) left_dx += dx - 1023; //Underflow

      else if(dx < -512) left_dx += dx + 1023; //Overflow

      else left_dx += dx;

    }

    /*Above 1023 is invalid data*/	

    if(!(right_data > 1023)) {

      right_data_array_put(right_data);

      //Adjust right accumulator

      dx = right_data - right_data_array_prev();

      if(right_data_array_prev()==0) dx=0;

      if(dx > 512) right_dx += dx - 1023; //underflow

      else if(dx < -512) right_dx += dx + 1023; //overflow 

      else right_dx += dx;

    }

  }

  //Increment timecount accumulator

  timecount++;

  return 0;