Colony

#include "encoders.h"

#include "spi.h"

#include <dragonfly_lib.h>

#include "ring_buffer.h"

unsigned int left_data_buf;

unsigned int right_data_buf;

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

int left_v;

int right_v;

volatile int recv_count;

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

//RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int);

void encoder_recv_complete(void);

void encoder_recv_complete(){

  int dx;

   //  usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n");

   //   usb_puts("\r\n");

  data_ready++;

  //delay_ms(ENCODER_DELAY);

   spi_transfer(5);

}

void put_bin(char data){

  int i;

  for(i=7;i>=0;i--)

    usb_puti((data>>i)&1);

  usb_puts(" ");

}

/**

 * Initializes the encoders variables

 **/

void encoders_init(void){

        int i;

        data_ready=0;

        spi_init(encoder_recv/*put_bin*/, encoder_recv_complete);

        buf_index = 0;

        left_data_buf = 0;

        right_data_buf= 0;

        left_data = -1;

        right_data = -1;

        recv_count=0;

        left_v=0;

        right_v=0;

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

}

/**

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

}

int encoder_change(char encoder){

        return 0;

}

char encoder_direction(char encoder){

        return 0;

}

/**

 * Gets the velocity of the specified encoder.

 *

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

 *

 * @return The velocity of 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 value of the velocity global variable for the specified

 *  encoder.

 *

 * @param encoder the encoder that you want to modify

 **/

void encoder_rst_dx(char encoder) {

        if(encoder==LEFT)

                left_dx = 0;

        else if(encoder==RIGHT)

                right_dx = 0;

}

/**

 * Returns the current time count for the encoders.

 **/

int encoder_get_tc(void) {

        return timecount;

}

/**

 * Resets the time count for the encoders.

 **/

void encoder_rst_tc(void) {

        timecount = 0;

}

int encoder_get_v(char encoder){

        int last, res=0;

        cli();

        if(encoder==LEFT){

                if(left_data_idx==0)

                        last = BUFFER_SIZE - 1;

                else

                        last = left_data_idx - 1;

                res = ((int)left_data_array[last]) - ((int)left_data_array[left_data_idx]);

        }

        if(encoder==RIGHT){

                if(right_data_idx==0)

                        last = BUFFER_SIZE - 1;

                else

                        last = right_data_idx - 1;

                res = ((int)right_data_array[right_data_idx]) - ((int)right_data_array[last]);

        }

        sei();

        while(res<MIN_V)//underflow

                res+=1024;

        while(res>MAX_V)//overflow

                res-=1024;

        return res;

}

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

        //usb_puti(buf_index);

        short int dx;

        recv_count++;

        //if(++recv_count==ENCODER_DELAY){

                recv_count=0;

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

                // second is offset by 1 bit.

                if(buf_index == 0)

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

                else if (buf_index == 1)

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

                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 |= (((short)data)>>7) & 0x1;

                buf_index = (buf_index + 1) % 5;

                if(buf_index==0) {

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

                                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 & (COF | LIN) || !(right_data_buf & OCF)){

                        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;

                  }

                        //if(left_data!=INVALID || right_data!=INVALID){

                        //  usb_puts("{");usb_puti(left_data);usb_puts(",");usb_puti(right_data);usb_puts("}\r\n");

                        //}

                          left_data_buf = 0;

                        right_data_buf = 0;

                if(left_data < INVALID) {   //Valid? I have no idea what being less than invalid means - KWoo

                        //Put new data onto data array

                        left_data_array_put(left_data);

                        //Adjust left accumulator

                        //dx = left_data - left_data_array_prev();

                        dx = left_data_array_prev() - left_data;

                        //Adjust velocity: save last dx

                        left_v = left_dx;

                        /*left dx was negative on robot 7, could just be that

                          the encoder is backwards, so this may need to change

                          back

                        */

                        if(left_data_array_prev()==0)

                          dx=0;

                        if(dx > 5) { //underflow

                                left_dx += dx - 1023;

                        }

                        else if(dx < -5) { //overflow

                                left_dx += dx + 1023;

                        }

                        else {

                                left_dx += dx;

                        }

                        //Adjust velocity: update

                        left_v = left_dx - left_v;

                }

                if(right_data < INVALID) {

                        //Put new data onto data array

                        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 > 5) { //underflow

                                right_dx += dx - 1023;

                        }

                        else if(dx < -5) { //overflow

                                right_dx += dx + 1023;

                        }

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

}