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 17

#define MAP_REQUEST_GROUP 23

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

void* buffer_data(void* arg);

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

        if(nrhs == 1){        

                /*Initialization*/

                head_index = BUFFER_EMPTY;

                tail_index = BUFFER_EMPTY;        

                pthread_rwlock_init(&buffer_lock, NULL);

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

                return;

        }

        /*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."); 

        }

        int index,i,j;

        pthread_rwlock_rdlock(&buffer_lock);

        {

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

                        double* point, *ranges, *theta, *robot;

                        /*Appropriately return with nothing.*/

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

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

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

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

                        point  = mxGetPr(ret[0]);

                        ranges = mxGetPr(ret[1]);

                        theta = mxGetPr(ret[2]);

                        robot = mxGetPr(ret[3]);

                        point[0] = point[1] = 0.0;

                        ranges[0] = ranges[1] = ranges[2] = 0.0;

                        ranges[3] = ranges[4] = 0.0;

                        theta[0] = 0.0;

                        robot[0] = 0.0;

                        return;

                }

                else{

                        double** point, **ranges, *theta, *robot;

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

                        /*This may not work on some computers.*/

                        point  = (double**)mxGetPr(ret[0]);

                        ranges = (double**)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);

}

/*

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

                        usleep(10000); /*10ms*/

                }

                /*Wait for all packets to be received 

                 * before sending more requests.

                 * FIXME this needs to be WAAAAY more robust

                 * to edge cases (robot enters / leaves token

                 * ring etc.) For example, if a robot drops

                 * a packet (highly likely) it is not dealt with

                 * */

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

        }

}