Revision 963
Minor modifications and fixes. Haven't gotten it to work yet.
| receive.c | ||
|---|---|---|
| 9 | 9 |
#define WL_COM_PORT "/dev/ttyUSB1" |
| 10 | 10 |
#define WL_CHANNEL 0xE |
| 11 | 11 |
|
| 12 |
#define MAP_RECEIVE_GROUP 17 |
|
| 13 | 12 |
#define MAP_REQUEST_GROUP 23 |
| 14 | 13 |
|
| 15 |
#define POINT_RAW 1 /* packet type for map data points w/ raw encoder data*/
|
|
| 16 |
#define POINT_ODO 2 /* packet type for map data points w/ odometry data*/
|
|
| 14 |
#define DATA_POINT 2 /* packet type for map data points w/ odometry data*/
|
|
| 15 |
#define DATA_REQUEST 3
|
|
| 17 | 16 |
|
| 18 | 17 |
#define BUFFER_SIZE 1000 |
| 19 | 18 |
#define BUFFER_EMPTY -1 |
| ... | ... | |
| 25 | 24 |
|
| 26 | 25 |
void packet_receive(char type, int source, unsigned char* packet, int length); |
| 27 | 26 |
|
| 28 |
PacketGroupHandler receiveHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL};
|
|
| 29 |
PacketGroupHandler requestHandler = {MAP_RECEIVE_GROUP, NULL, NULL, NULL, NULL};
|
|
| 27 |
PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL};
|
|
| 30 | 28 |
|
| 31 | 29 |
/*Buffers*/ |
| 32 | 30 |
short robot_buffer[BUFFER_SIZE]; /*by #*/ |
| ... | ... | |
| 129 | 127 |
* waits for them to respond. |
| 130 | 128 |
*/ |
| 131 | 129 |
void* buffer_data(void* arg){
|
| 132 |
int robot_num, num_robots; |
|
| 130 |
int robot_num, num_robots,time_out, i;
|
|
| 133 | 131 |
|
| 134 | 132 |
pthread_detach(pthread_self()); |
| 135 | 133 |
|
| ... | ... | |
| 137 | 135 |
wl_set_com_port(WL_COM_PORT); |
| 138 | 136 |
wl_init(); |
| 139 | 137 |
wl_set_channel(WL_CHANNEL); |
| 140 |
wl_register_packet_group(&receiveHandler);
|
|
| 138 |
wl_register_packet_group(&requestHandler);
|
|
| 141 | 139 |
|
| 142 | 140 |
/*Token ring / sensor network initialization.*/ |
| 143 | 141 |
wl_token_ring_register(); |
| ... | ... | |
| 145 | 143 |
/*Continuously iterate over all the robots - buffering data.*/ |
| 146 | 144 |
|
| 147 | 145 |
while(1){
|
| 148 |
|
|
| 146 |
wl_do(); |
|
| 147 |
|
|
| 148 |
printf("sending new round of packets\n");
|
|
| 149 | 149 |
wl_token_iterator_begin(); |
| 150 | 150 |
|
| 151 | 151 |
num_robots = wl_token_get_num_robots(); |
| ... | ... | |
| 155 | 155 |
|
| 156 | 156 |
/*Send a data request to all robots.*/ |
| 157 | 157 |
while(wl_token_iterator_has_next()){
|
| 158 |
wl_do(); |
|
| 159 |
|
|
| 158 | 160 |
robot_num = wl_token_iterator_next(); |
| 159 | 161 |
|
| 162 |
printf("sending a packet to robot %d\n",robot_num);
|
|
| 160 | 163 |
wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
| 161 |
POINT_ODO, NULL, 0, robot_num, 0);
|
|
| 164 |
DATA_REQUEST, NULL, 0, robot_num, 0);
|
|
| 162 | 165 |
|
| 163 |
usleep(10000); /*10ms*/ |
|
| 166 |
for(i=0;i<50;i++){
|
|
| 167 |
wl_do(); |
|
| 168 |
usleep(10000); /*500ms*/ |
|
| 169 |
} |
|
| 164 | 170 |
} |
| 165 | 171 |
|
| 172 |
time_out = 50; /*50 * 10ms = 1s*/ |
|
| 173 |
|
|
| 166 | 174 |
/*Wait for all packets to be received |
| 167 | 175 |
* before sending more requests. |
| 168 |
* FIXME this needs to be WAAAAY more robust |
|
| 169 |
* to edge cases (robot enters / leaves token |
|
| 170 | 176 |
* ring etc.) For example, if a robot drops |
| 171 | 177 |
* a packet (highly likely) it is not dealt with |
| 172 | 178 |
* */ |
| 173 | 179 |
while(packets_received < num_robots |
| 174 |
&& packets_received < wl_token_get_num_robots()){
|
|
| 180 |
&& packets_received < wl_token_get_num_robots() && time_out){
|
|
| 175 | 181 |
|
| 182 |
wl_do(); |
|
| 183 |
time_out--; |
|
| 176 | 184 |
usleep(10000); /*10ms*/ |
| 177 | 185 |
} |
| 178 |
|
|
| 179 |
usleep(10000); |
|
| 186 |
|
|
| 187 |
for(i=0;i<50;i++){
|
|
| 188 |
wl_do(); |
|
| 189 |
usleep(10000); /*500ms*/ |
|
| 190 |
} |
|
| 180 | 191 |
} |
| 181 | 192 |
} |
| 182 | 193 |
|
| 183 | 194 |
void packet_receive (char type, int source, unsigned char* packet, int length) {
|
| 184 | 195 |
printf("A packet came in!\n");
|
| 185 |
if(type==POINT_ODO){
|
|
| 196 |
if(type==DATA_POINT){
|
|
| 186 | 197 |
|
| 187 | 198 |
if(length!=18) printf("Packet came without the standard length!\n");
|
| 188 | 199 |
|
Also available in: Unified diff