Colony

#include "encoders.h"

#include "spi.h"

#include <dragonfly_lib.h>

#include "ring_buffer.h"

unsigned short int left_data_buf;

unsigned short int right_data_buf;

char buf_index;

unsigned short int left_data;

unsigned short int right_data;

RING_BUFFER_NEW(enc_buffer, 16, short int, spi_send_buff, spi_rec_buff);

void encoders_init(void){

        spi_init(encoder_recv);

        buf_index = 0;

        left_data_buf = 0;

        right_data_buf= 0;

        left_data = -1;

        right_data = -1;

}

int encoder_read(char encoder){

        if(encoder==LEFT)

                return left_data;

        else if(encoder==RIGHT)

                return right_data;

        else return -1;

}

int encoder_change(char encoder){

        return 0;

}

char encoder_direction(char encoder){

        return 0;

}

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

//every 8 microseconds.

void encoder_recv(char data){

        if(buf_index < 2)

                right_data_buf |= (((short)data) << ((1-buf_index)*8)) & (0xF<<(1-buf_index));

        else if (buf_index == 2)

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

        else if (buf_index == 3)

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

        else if (buf_index == 4)

                left_data_buf |= (data>>7) & 0x1;

        buf_index = (buf_index + 1) % 5;

        if(buf_index==0){

                if(left_data_buf & (OCF | COF | LIN)) 

                        left_data = INVALID;

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

                        left_data = MAGNET_FAILURE;

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

                if(right_data_buf & (OCF | COF | LIN)) 

                        right_data = INVALID;

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

                        left_data = MAGNET_FAILURE;

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

                left_data_buf = 0;

                right_data_buf = 0;

        }

}