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 "spi.h"

#include "ring_buffer.h"

#include "encoders.h"

unsigned char encoders_initd=0;

unsigned int left_data_buf;

unsigned int right_data_buf;

char encoder_buf_index;

unsigned int left_data;

unsigned int right_data;

unsigned int left_data_array[BUFFER_SIZE];

unsigned int right_data_array[BUFFER_SIZE];

int left_data_idx;

int right_data_idx;

int left_dx;

int right_dx;

long int timecount;

volatile short int data_ready;

int encoder_recv(char data);

//Helper Function Prototypes

inline void left_data_array_put(unsigned short int value);

inline unsigned int left_data_array_top(void);

inline unsigned int left_data_array_prev(void);

inline unsigned int left_data_array_bottom(void);

inline void right_data_array_put(unsigned short int value);

inline unsigned int right_data_array_top(void);

inline unsigned int right_data_array_prev(void);

inline unsigned int right_data_array_bottom(void);

//encoder_get_v helper function prototypes

int left_data_at(int index);

int right_data_at(int index);

int get_dx(char encoder, int index);

void encoder_recv_complete(void){

          encoder_buf_index = 0;

        data_ready++;

           spi_transfer(5);

}

/**

* @brief Initializes encoder variables and the hardware interface.

*/

int encoders_init(void){

  int i;

  if(encoders_initd) {

    DRAGONFLY_DEBUG_PRINT("ERROR: encoders already init'd\r\n");

    return ERROR_INIT_ALREADY_INITD;

  }

  data_ready=0;

  if(spi_init(encoder_recv, encoder_recv_complete)) {

    DRAGONFLY_DEBUG_PRINT("ERROR: SPI was already init'd, bailing out\r\n");

    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;

}

/**

 * @brief Returns the specified encoders value

 *

 * @param encoder this is the encoder that you want to read. Valid arguments

 *          are LEFT and RIGHT

 *

 * @return the value of the specified encoder.

 *         -1 usually means low battery.

 *         -2 means the library was not properly initialized

 *         values above ENCODER_MAX usually means phyiscal problems with

 *         the encoder.

 **/

int encoder_read(char encoder){

  if(!encoders_initd) {

    DRAGONFLY_DEBUG_PRINT("ERROR: encoders not init'd\r\n");

    return -2;

  }

  if(encoder==LEFT) {

    return left_data;

  }

  else if(encoder==RIGHT){

    return right_data;

  }

  else{

    return -1;

  }

}

/**

 * @brief Get total distance travelled by the specified encoder (in encoder ticks)

 *

 * @param encoder the encoder that you want to read, either LEFT or RIGHT

 *

 * @return The distance covered by the specified encoder.

 *

 * @note Simply calls encoder_get_dx.

 **/

int encoder_get_x(char encoder) {

        return encoder_get_dx(encoder);

}

/**

 * Gets the total distance covered by the specified encoder (in encoder clicks)

 *

 * @param encoder the encoder that you want to read, use LEFT or RIGHT

 *

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

 **/

int encoder_get_dx(char encoder) {

  if(!encoders_initd)

    return -2;

  if(encoder==LEFT)

    return left_dx;

  else if(encoder==RIGHT)

    return right_dx;

  else

    return -1;

}

/**

 * @brief Returns the approximated instantaneous velocity of the robot

 * in terms of encoder clicks.

 *

 * @param encoder RIGHT or LEFT - the wheel you want the velocity for.

 *

 * @return The instantaneous velocity for the given wheel or twice the ERR_VEL

 *         if an error occurs (1024 * 2 = 2048)

 *

 * @bug This uses hard coded values and results are inconsistent.

 *      Use at your own risk.

 */

int encoder_get_v(char encoder){

    int vel1, vel2;

    vel1 = get_dx(encoder, 0);

    vel2 = get_dx(encoder, 1);

    if (vel1 == ERR_VEL && vel2 == ERR_VEL)

        return ERR_VEL << 1;

    else if (vel2 == ERR_VEL)

        return vel1 << 1;

    else if (vel1 == ERR_VEL)

        return vel2 << 1;

    else

        return vel1 + vel2;

}

/**

 * Resets the distance accumulator for the specified

 *  encoder.

 *

 * @param encoder the encoder that you want to reset distance for

 **/

int encoder_rst_dx(char encoder) {

  if(!encoders_initd) {

    DRAGONFLY_DEBUG_PRINT("ERROR: encoders not init'd\r\n");

    return ERROR_LIBRARY_NOT_INITD;

  }

  if(encoder==LEFT)

    left_dx = 0;

  else if(encoder==RIGHT)

    right_dx = 0;

  return 0;

}

/**

* @brief Returns the number of encoder reads that have occurred.

*

* @return The time count.

*/

int encoder_get_tc(void) {

        return timecount;

}

/**

* @brief Resets the encoder read counter.

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

*/

int encoder_rst_tc(void) {

  if(!encoders_initd) {

    DRAGONFLY_DEBUG_PRINT("ERROR: encoders not init'd\r\n");

    return ERROR_LIBRARY_NOT_INITD;

  }

  timecount = 0;

  return 0;

}

/**

* @brief Waits until n encoder reads have occurred.

* Counter is reset on functions exit.

*

* @param n

*

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

*/

int encoder_wait(int n){

  if(!encoders_initd) {

    DRAGONFLY_DEBUG_PRINT("ERROR: encoders not init'd\r\n");

    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) {

    DRAGONFLY_DEBUG_PRINT("ERROR: encoders not init'd\r\n");

    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;

}

//Helper Functions

inline void left_data_array_put(unsigned short int value) {

        if(left_data_idx == BUFFER_SIZE-1)

                left_data_idx = 0;

        else

                left_data_idx++;

        left_data_array[left_data_idx] = value;

}

inline unsigned int left_data_array_top(void) {

        return left_data_array[left_data_idx];

}

inline unsigned int left_data_array_prev(void) {

        if(left_data_idx == 0)

                return left_data_array[BUFFER_SIZE-1];

        else

                return left_data_array[left_data_idx - 1];

}

inline unsigned int left_data_array_bottom(void) {

        if(left_data_idx == BUFFER_SIZE-1)

                return left_data_array[0];

        else

                return left_data_array[left_data_idx + 1];

}

inline void right_data_array_put(unsigned short int value) {

        if(right_data_idx == BUFFER_SIZE-1)

                right_data_idx = 0;

        else

                right_data_idx++;

        right_data_array[right_data_idx] = value;

}

inline unsigned int right_data_array_top(void) {

        return right_data_array[right_data_idx];

}

inline unsigned int right_data_array_prev(void) {

        if(right_data_idx == 0)

                return right_data_array[BUFFER_SIZE-1];

        else

                return right_data_array[right_data_idx - 1];

}

inline unsigned int right_data_array_bottom(void) {

        if(right_data_idx == BUFFER_SIZE-1)

                return right_data_array[0];

        else

                return right_data_array[right_data_idx + 1];

}

/* Helper functions for encoder_get_v */

int left_data_at(int index) {

    int tmp_idx = left_data_idx - index;

    if (tmp_idx < 0)

        tmp_idx += BUFFER_SIZE;

    return left_data_array[tmp_idx];

}

int right_data_at(int index) {

    int tmp_idx = right_data_idx - index;

    if (tmp_idx < 0)

        tmp_idx += BUFFER_SIZE;

    return right_data_array[tmp_idx];

}

int get_dx(char encoder, int index) {

    int dx, ctr;

    ctr = 0;

    dx = 1024;

    do {

        if (encoder == LEFT)

            dx = left_data_at(index+ctr) - left_data_at(index+ctr+38);

        else

            dx = right_data_at(index+ctr) - right_data_at(index+ctr+38);

        ctr++;

    } while ((dx > 30 || dx < -30) && ctr < 3);

    if (dx > 30 || dx < -30)

        return ERR_VEL;

    return dx;

}