root / branches / slam / code / projects / colonet / DataRequests / robot / data_response.c @ 124
History | View | Annotate | Download (1.58 KB)
1 |
#include "data_response.h" |
---|---|
2 |
#include <dragonfly_lib.h> |
3 |
#include "wl_token_ring.h" |
4 |
#include "wireless.h" |
5 |
|
6 |
|
7 |
PacketGroupHandler pgh; |
8 |
|
9 |
void handle_receive(char type, int source, unsigned char * packet, int length); |
10 |
|
11 |
int generate_bom_packet(char** buf); |
12 |
int generate_IR_packet(char** buf); |
13 |
|
14 |
void data_request_init(void){ |
15 |
range_init(); |
16 |
pgh.timeout_handler = NULL;
|
17 |
pgh.handle_response = NULL;
|
18 |
pgh.handle_response = handle_receive; |
19 |
pgh.unregister = NULL;
|
20 |
} |
21 |
|
22 |
void handle_receive(char type, int source, unsigned char* packet, int length){ |
23 |
char buf[MAX_PACKET_LENGTH];
|
24 |
int size;
|
25 |
switch(type){
|
26 |
case BOM_TYPE:
|
27 |
size = generate_bom_packet(&buf); |
28 |
wl_send_robot_to_robot_packet(DATA_REQUEST_GROUP,BOM_TYPE,data,size,source,0);
|
29 |
case IR_TYPE:
|
30 |
size = generate_IR_packet(&buf); |
31 |
wl_send_robot_to_robot_packet(DATA_REQUEST_GROUP,IR_TYPE,data,size,source,0);
|
32 |
case ENCODER_TYPE:
|
33 |
//Not yet supported.
|
34 |
|
35 |
} |
36 |
} |
37 |
|
38 |
int generate_bom_packet(char** buf){ |
39 |
short int i=0, robot_id; |
40 |
char bom_data;
|
41 |
wl_token_iterator_begin(); |
42 |
while(wl_token_iterator_has_next() && i<=99){ |
43 |
robot_id = wl_token_iterator_next(); |
44 |
*buf[i++] = (char)((robot_id>>8) & 255); |
45 |
*buf[i++] = (char)(robot_id & 255); |
46 |
*buf[i++] = (char)(wl_token_get_my_sensor_reading(robot_id));
|
47 |
} |
48 |
//i is one greater than the index of the last byte... the size.
|
49 |
return i;
|
50 |
} |
51 |
|
52 |
int generate_IR_packet(char** buf){ |
53 |
*buf[0] = range_read_distance(IR1);
|
54 |
*buf[1] = range_read_distance(IR2);
|
55 |
*buf[2] = range_read_distance(IR3);
|
56 |
*buf[3] = range_read_distance(IR4);
|
57 |
*buf[4] = range_read_distance(IR5);
|
58 |
return 5; |
59 |
} |