Colony

#include <stdio.h>

#include <stdlib.h>

//#include <mex.h>

#include <pthread.h>

#include <unistd.h>

#include "../../libwireless/lib/wireless.h"

#include "../../libwireless/lib/wl_token_ring.h"

#define WL_COM_PORT "/dev/ttyUSB1" 

#define WL_CHANNEL 0xE

#define MAP_RECEIVE_GROUP 1

#define MAP_REQUEST_GROUP 2

#define POINT_RAW 1 // packet type for map data points w/ raw encoder data

#define POINT_ODO 2 // packet type for map data points w/ odometry data

#define BUFFER_SIZE 1000

#define BUFFER_EMPTY -1

pthread_rwlock_t buffer_lock;

pthread_t receive_thread;

int packets_received; //Used to block overlapping packet requests.

void packet_receive(char type, int source, unsigned char* packet, int length);

PacketGroupHandler receiveHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL};

PacketGroupHandler requestHandler = {MAP_RECEIVE_GROUP, NULL, NULL, NULL, NULL};

//Buffers

short robot_buffer[BUFFER_SIZE];                        //by #

short range_buffer[BUFFER_SIZE][5];                 //IR1, IR2 ... 

short coordinate_buffer[BUFFER_SIZE][2];        //X Y

float orientation_buffer[BUFFER_SIZE];

int tail_index; //Points to the newest data in the buffers.

int head_index; //Points to the oldest data in the buffers.

int receive_initialized = 0;

void* buffer_data(void* arg);

//void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){

int main(){        

        if(!receive_initialized){        

                //Initialization

                head_index = BUFFER_EMPTY;

                tail_index = BUFFER_EMPTY;        

                pthread_rwlock_init(&buffer_lock, NULL);

                pthread_create(&receive_thread, NULL, buffer_data, NULL);

                receive_initialized = 1;

        }

        while(1){}

        /*Return stuff to matlab

        //deliberately ignore nrhs.        

        if(nlhs != 4){

                mexErrMsgTxt("Error: Expect output arguments [P IR T R]\n 

                                          where P = [X Y], IR = [IR1, IR2 ...],\n

                                          T = angle, and R = Robot number."); 

        }

        double* point, ranges, theta;

        int index,i,j;

        pthread_rwlock_rdlock(&buffer_lock);

        {

                if(tail_index == BUFFER_EMPTY || head_index == BUFFER_EMPTY){

                        //Appropriately return with nothing.

                }

                else{

                        //TODO Check for the 'already empty' case.

                        int num_buffer_elements = tail_index - head_index;

                        if(num_buffer_elements < 0) num_buffer_elements += BUFFER_SIZE;        

                        ret[0] = mxCreateDoubleMatrix(num_buffer_elements, 2, mxREAL);

                        ret[1] = mxCreateDoubleMatrix(num_buffer_elements, 5, mxREAL);

                        ret[2] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL);

                        ret[3] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL);

                        point  = mxGetPr(ret[0]);

                        ranges = mxGetPr(ret[1]);

                        theta = mxGetPr(ret[2]);

                        robot = mxGetPr(ret[3]);

                        for(index = head_index; index <= tail_index; index = (index + 1)%1024){

                                point[i][0] = (double)coordinate_buffer[index][0];        

                                point[i][1] = (double)coordinate_buffer[index][1];        

                                for(j=0;j<5;j++)

                                        ranges[i][j] = (double)range_buffer[index][j];

                                theta[i] = (double)(orientation_buffer[index]);        

                                robot[i] = (double)(robot_buffer[index]);

                        }

                        head_index = BUFFER_EMPTY;

                        tail_index = BUFFER_EMPTY;

        }        

        pthread_rwlock_unlock(&buffer_lock);

        */

        return -1;

}

/*

 * buffer_data continuously runs through the robots and

 * waits for them to respond.

 */

void* buffer_data(void* arg){

        int robot_num, num_robots;        

        pthread_detach(pthread_self());

        //General communication initialization.        

        wl_set_com_port(WL_COM_PORT);        

        wl_init();

        wl_set_channel(WL_CHANNEL);

        wl_register_packet_group(&receiveHandler);

        //Token ring / sensor network initialization.

        wl_token_ring_register();        

        //Continuously iterate over all the robots - buffering data.

        while(1){

                wl_token_iterator_begin();

                num_robots = wl_token_get_num_robots();

                //Initialize the 'packets received' number. 

                packets_received = 0;

                //Send a data request to all robots.

                while(wl_token_iterator_has_next()){

                        robot_num = wl_token_iterator_next();

                        wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, 

                                POINT_ODO, NULL, 0, robot_num, 0);

                }

                //Wait for all packets to be received before sending more requests.

                while(packets_received < num_robots 

                   && packets_received < wl_token_get_num_robots()){

                        usleep(10000); //10ms

                }

                usleep(10000); 

        }

}

void packet_receive (char type, int source, unsigned char* packet, int length) {

        printf("A packet came in!\n");        

        if(type==POINT_ODO){        

                if(length!=18) printf("Packet came without the standard length!\n");

                // expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5

                // Pull in data:

                short x,y,i;

                short ranges[5];        

                x = ((short)packet[1] << 8)  | (short)packet[0];

                y = ((short)packet[3] << 8)  | (short)packet[2];

                for(i=0; i<5; i++){

                        ranges[i] 

                           = ((short)packet[8+2*i+1] << 8) | (short)packet[8 + 2*i];

                }

                float theta = *((float*)(packet + 4));

                //Write new data to buffers. (Lock down!)

                pthread_rwlock_wrlock(&buffer_lock);

                {        

                        //Note that you write no matter what.        

                        tail_index = (tail_index + 1) % BUFFER_SIZE; 

                        //Write the robots number.

                        robot_buffer[tail_index] = source;

                        //Write in range data.

                        for(i=0; i<5; i++) range_buffer[tail_index][i] = ranges[i];

                        coordinate_buffer[tail_index][0] = x;

                        coordinate_buffer[tail_index][1] = y;

                        orientation_buffer[tail_index] = theta;

                        //If the oldest member didn't exist, buffer was empty.

                        if(head_index == BUFFER_EMPTY) head_index = 0;

                        //Otherwise check for overwritten data.        

                        else if(tail_index == head_index) 

                                head_index = (head_index +1) % BUFFER_SIZE;  

                }

                pthread_rwlock_unlock(&buffer_lock);

                //Increment for the transmit thread.

                packets_received++;

        }

}