Colony

#include "encoders.h"

#include "spi.h"

#include "ring_buffer.h"

#include <avr/io.h>

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;

void 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.

*/

void encoders_init(void){

        int i;

        data_ready=0;

        spi_init(encoder_recv, encoder_recv_complete);

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

}

/**

 * @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.

 *         values above ENCODER_MAX usually means phyiscal problems with

 *         the encoder.

 **/

int encoder_read(char encoder){

        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.

 **/

int encoder_get_dx(char encoder) {

        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

 **/

void encoder_rst_dx(char encoder) {

        if(encoder==LEFT) left_dx = 0;

        else if(encoder==RIGHT) right_dx = 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.

*/

void encoder_rst_tc(void) {

        timecount = 0;

}

/**

* @brief Waits until n encoder reads have occurred.

* Counter is reset on functions exit.

*

* @param n

*/

void encoder_wait(int n){

        while(data_ready<n);

        data_ready=0;

}

//Full reads occur every 40 microseconds. This function should be called

//every 8 microseconds.

void encoder_recv(char data){

        short int dx;

        //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++;

}

//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;

}