Colony

/* This version of receive initializes

 * the token ring and then waits for

 * the robots to send data.

 */

#include <stdio.h>

#include <stdlib.h>

#include <mex.h>

#include <pthread.h>

#include <unistd.h>

#include <time.h>

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

#define WL_COM_PORT "/dev/ttyUSB0" 

#define WL_CHANNEL 0xE

#define MAP_REQUEST_GROUP 23

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

#define DATA_REQUEST 3

#define DATA_SERVER_REPLY 4 /*Just to give the robot a target.*/

#define DATA_SERVER_IDENTIFY 5

#define BUFFER_SIZE 1000

pthread_rwlock_t buffer_lock;

pthread_t receive_thread;

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

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

struct robot_datapoint{

        time_t time;

        short number;

        short range[5];

        short x;

        short y;

        float orientation;

} robot_data[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* run_wireless(void* arg);

void init_wireless(void);

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

        if(nrhs == 1){        

                /*Initialization*/

                head_index = -1;

                tail_index = -1;        

                pthread_rwlock_init(&buffer_lock, NULL);

                pthread_create(&receive_thread, NULL, run_wireless, 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 target, index,i,j;

        if(tail_index == -1 || head_index == -1){

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

                /*Appropriately return with nothing. (til I figure that out, return 0's)*/

                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;

        }

        else{ /*Copy the buffer data and set the buffer to empty.*/

                pthread_rwlock_rdlock(&buffer_lock);

                {

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

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

                        ranges = (double**)mxGetPr(ret[1]);

                        theta = mxGetPr(ret[2]);

                        robot = mxGetPr(ret[3]);

                        for(index = 0; index < num_buffer_elements; index++){

                                int target = (head_index + index) % BUFFER_SIZE;        

                                point[i][0] = (double)robot_data[target].x;        

                                point[i][1] = (double)robot_data[target].y;        

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

                                        ranges[i][j] = (double)robot_data[target].range[j];

                                theta[i] = (double)(robot_data[target].orientation);        

                                robot[i] = (double)(robot_data[target].number);

                        }

                        /*Relabel the indices 'empty' (-1)*/

                        head_index = -1;

                        tail_index = -1;

                }

                pthread_rwlock_unlock(&buffer_lock);

        }

}

void init_wireless(void){

        /*General communication initialization.        */

        wl_set_com_port(WL_COM_PORT);        

        wl_init();

        wl_set_channel(WL_CHANNEL);

        wl_register_packet_group(&requestHandler);

}

/*Call wl_do and occasionally send a broadcast packet declaring the 

 * number of the server.

 */

void* run_wireless(void* arg){

        pthread_detach(pthread_self());

        init_wireless();

        printf("Wireless initialized!\n");

        int a = 0;

        /*Passively wait for packets to arrive.*/

        while(1) wl_do();

}

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

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

        if(type==DATA_POINT){        

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

                /*Bytes 0-3*/

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

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

                /*Bytes 4-7*/

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

                /*Bytes 8-17*/

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

                        ranges[i] 

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

                }

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

                pthread_rwlock_wrlock(&buffer_lock);

                {        

                        /*Note that you write no matter what.

                        * Also, if the buffer is empty, tail_index becomes 0*/

                        tail_index = (tail_index + 1) % BUFFER_SIZE; 

                        /*Write the robots number.*/

                        robot_data[tail_index].number = source;

                        /*Write in range data.*/

                        for(i=0; i<5; i++) robot_data[tail_index].range[i] = ranges[i];

                        robot_data[tail_index].x = x;

                        robot_data[tail_index].y = y;

                        robot_data[tail_index].orientation = theta;

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

                        if(head_index == -1) 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);

        }

        else if(type == DATA_SERVER_IDENTIFY){

                /*Sleep before sending an identity packet.*/

                printf("Announcing self as server\n");

                usleep(5000);

                wl_send_global_packet(MAP_REQUEST_GROUP, DATA_SERVER_REPLY, NULL, 0, 0);

                usleep(5000);

        }

}