Revision 1028
Changes all around. Matlab server is now passive, might work soon.
| receive.c | ||
|---|---|---|
| 12 | 12 |
#include "../../libwireless/lib/wireless.h" |
| 13 | 13 |
#include "../../libwireless/lib/wl_token_ring.h" |
| 14 | 14 |
|
| 15 |
#define WL_COM_PORT "/dev/ttyUSB1"
|
|
| 15 |
#define WL_COM_PORT "/dev/ttyUSB0"
|
|
| 16 | 16 |
#define WL_CHANNEL 0xE |
| 17 | 17 |
|
| 18 | 18 |
#define MAP_REQUEST_GROUP 23 |
| 19 | 19 |
|
| 20 | 20 |
#define DATA_POINT 2 /* packet type for map data points w/ odometry data*/ |
| 21 | 21 |
#define DATA_REQUEST 3 |
| 22 |
#define DATA_INIT 4 /*Just to give the robot a target.*/ |
|
| 22 | 23 |
|
| 23 | 24 |
#define BUFFER_SIZE 1000 |
| 24 | 25 |
#define BUFFER_EMPTY -1 |
| ... | ... | |
| 26 | 27 |
pthread_rwlock_t buffer_lock; |
| 27 | 28 |
pthread_t receive_thread; |
| 28 | 29 |
|
| 29 |
int packets_received; /*Used to block overlapping packet requests.*/ |
|
| 30 |
|
|
| 31 | 30 |
void packet_receive(char type, int source, unsigned char* packet, int length); |
| 32 | 31 |
|
| 33 | 32 |
PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL};
|
| ... | ... | |
| 41 | 40 |
int tail_index; /*Points to the newest data in the buffers.*/ |
| 42 | 41 |
int head_index; /*Points to the oldest data in the buffers.*/ |
| 43 | 42 |
|
| 44 |
void* buffer_data(void* arg);
|
|
| 43 |
void* run_wireless(void* arg);
|
|
| 45 | 44 |
void init_wireless(void); |
| 46 | 45 |
|
| 47 | 46 |
void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){
|
| ... | ... | |
| 54 | 53 |
|
| 55 | 54 |
pthread_rwlock_init(&buffer_lock, NULL); |
| 56 | 55 |
|
| 57 |
pthread_create(&receive_thread, NULL, buffer_data, NULL);
|
|
| 56 |
pthread_create(&receive_thread, NULL, run_wireless, NULL);
|
|
| 58 | 57 |
return; |
| 59 | 58 |
} |
| 60 | 59 |
|
| ... | ... | |
| 74 | 73 |
|
| 75 | 74 |
if(tail_index == BUFFER_EMPTY || head_index == BUFFER_EMPTY){
|
| 76 | 75 |
double* point, *ranges, *theta, *robot; |
| 77 |
/*Appropriately return with nothing.*/ |
|
| 76 |
/*Appropriately return with nothing. (til I figure that out, return 0's)*/
|
|
| 78 | 77 |
ret[0] = mxCreateDoubleMatrix(1, 2, mxREAL); |
| 79 | 78 |
ret[1] = mxCreateDoubleMatrix(1, 5, mxREAL); |
| 80 | 79 |
ret[2] = mxCreateDoubleMatrix(1, 1, mxREAL); |
| ... | ... | |
| 93 | 92 |
theta[0] = 0.0; |
| 94 | 93 |
robot[0] = 0.0; |
| 95 | 94 |
|
| 96 |
return; |
|
| 97 | 95 |
} |
| 98 |
else{
|
|
| 96 |
else{ /*Copy the buffer data and set the buffer to empty.*/
|
|
| 99 | 97 |
double** point, **ranges, *theta, *robot; |
| 100 | 98 |
|
| 101 | 99 |
int num_buffer_elements = tail_index - head_index; |
| ... | ... | |
| 106 | 104 |
ret[2] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL); |
| 107 | 105 |
ret[3] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL); |
| 108 | 106 |
|
| 109 |
/*This may not work on some computers.*/ |
|
| 110 | 107 |
point = (double**)mxGetPr(ret[0]); |
| 111 | 108 |
ranges = (double**)mxGetPr(ret[1]); |
| 112 | 109 |
theta = mxGetPr(ret[2]); |
| ... | ... | |
| 140 | 137 |
wl_token_ring_register(); |
| 141 | 138 |
} |
| 142 | 139 |
|
| 143 |
/* |
|
| 144 |
* buffer_data continuously runs through the robots and
|
|
| 145 |
* waits for them to respond.
|
|
| 140 |
|
|
| 141 |
/*Call wl_do and occasionally send a broadcast packet declaring the
|
|
| 142 |
* number of the server.
|
|
| 146 | 143 |
*/ |
| 147 |
void* buffer_data(void* arg){
|
|
| 144 |
void* run_wireless(void* arg){
|
|
| 148 | 145 |
int robot_num, num_robots,time_out, i; |
| 149 | 146 |
|
| 150 | 147 |
pthread_detach(pthread_self()); |
| 151 | 148 |
|
| 152 | 149 |
init_wireless(); |
| 150 |
printf("Wireless initialized\n");
|
|
| 153 | 151 |
|
| 154 |
/*Continuously iterate over all the robots - buffering data.*/ |
|
| 155 |
|
|
| 152 |
int a = 0; |
|
| 156 | 153 |
while(1){
|
| 154 |
/*Passively wait for packets to arrive.*/ |
|
| 157 | 155 |
wl_do(); |
| 158 |
|
|
| 159 |
printf("sending new round of packets\n");
|
|
| 160 |
wl_token_iterator_begin(); |
|
| 161 |
|
|
| 162 |
num_robots = wl_token_get_num_robots(); |
|
| 163 |
|
|
| 164 |
/*Initialize the 'packets received' number. */ |
|
| 165 |
packets_received = 0; |
|
| 166 |
|
|
| 167 |
/*Send a data request to all robots.*/ |
|
| 168 |
while(wl_token_iterator_has_next()){
|
|
| 169 |
wl_do(); |
|
| 170 |
|
|
| 171 |
robot_num = wl_token_iterator_next(); |
|
| 172 |
|
|
| 173 |
printf("sending a packet to robot %d\n",robot_num);
|
|
| 174 |
wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
|
| 175 |
DATA_REQUEST, NULL, 0, robot_num, 0); |
|
| 176 |
|
|
| 177 |
for(i=0;i<50;i++){
|
|
| 178 |
wl_do(); |
|
| 179 |
usleep(10000); /*500ms*/ |
|
| 180 |
} |
|
| 181 |
} |
|
| 182 |
|
|
| 183 |
time_out = 50; /*50 * 10ms = 1s*/ |
|
| 184 |
|
|
| 185 |
/*Wait for all packets to be received |
|
| 186 |
* before sending more requests. |
|
| 187 |
* ring etc.) For example, if a robot drops |
|
| 188 |
* a packet (highly likely) it is not dealt with |
|
| 189 |
* */ |
|
| 190 |
while(packets_received < num_robots |
|
| 191 |
&& packets_received < wl_token_get_num_robots() && time_out){
|
|
| 192 |
|
|
| 193 |
wl_do(); |
|
| 194 |
time_out--; |
|
| 195 |
usleep(10000); /*10ms*/ |
|
| 196 |
} |
|
| 197 |
|
|
| 198 |
for(i=0;i<50;i++){
|
|
| 199 |
wl_do(); |
|
| 200 |
usleep(10000); /*500ms*/ |
|
| 201 |
} |
|
| 202 | 156 |
} |
| 203 | 157 |
} |
| 204 | 158 |
|
| ... | ... | |
| 206 | 160 |
printf("A packet came in!\n");
|
| 207 | 161 |
if(type==DATA_POINT){
|
| 208 | 162 |
|
| 209 |
if(length!=18) printf("Packet came without the standard length!\n");
|
|
| 163 |
if(length!=18) |
|
| 164 |
printf("Packet came without the standard length!\n");
|
|
| 210 | 165 |
|
| 211 | 166 |
/* expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5*/ |
| 212 | 167 |
/* Pull in data:*/ |
| ... | ... | |
| 250 | 205 |
|
| 251 | 206 |
} |
| 252 | 207 |
pthread_rwlock_unlock(&buffer_lock); |
| 253 |
|
|
| 254 |
/*Increment for the transmit thread.*/ |
|
| 255 |
packets_received++; |
|
| 256 | 208 |
} |
| 209 |
if(type == DATA_INIT){
|
|
| 210 |
wl_send_global_packet(MAP_REQUEST_GROUP, DATA_INIT, NULL, 0, 0); |
|
| 211 |
printf("Announcing self as server\n");
|
|
| 212 |
} |
|
| 257 | 213 |
} |
| 258 | 214 |
|
Also available in: Unified diff