Colony

#include "encoders.h"

char encoder_buf_index;

unsigned int left_data, 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(unsigned int, unsigned int);

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

/**

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

 **/

int encoder_read(char encoder){

        if(encoder==LEFT) return left_data;

        else if(encoder==RIGHT) return right_data;

        else return -1;

}

/**

* @brief Outputs encoder direction as FORWARD OR BACK

* A STUB! DO NOT use.

*

* @param encoder The encoder you want the direction of.

* Valid arguments are right and left.

*

* @return FORWARD or BACK (the constants)

*/

char encoder_direction(char encoder){

        return 0;

}

/**

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

}

/**

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

*/

int encoder_get_v(char encoder){

        if (encoder == LEFT)

                return left_data_array_bottom() - left_data_array_top();

        if (encoder == RIGHT)

                return right_data_array_top() - right_data_array_bottom();

        return -1; /* TODO: velocity could be -1, use another value for error */

}

/**

* @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(unsigned int left_data_buf, unsigned int right_data_buf){

        short int dx;

        left_data = left_data_buf;

        right_data = right_data_buf;

        /*Above 1023 is invalid data*/

        if(!(left_data > 1023)) {

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

}