root / branches / slam / code / projects / colonet / DataRequests / robot / data_response.c @ 157
History | View | Annotate | Download (1.63 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_receive = handle_receive; |
19 |
pgh.unregister = NULL;
|
20 |
wl_register_packet_group(&pgh); |
21 |
} |
22 |
|
23 |
void handle_receive(char type, int source, unsigned char* packet, int length){ |
24 |
char buf[MAX_PACKET_LENGTH];
|
25 |
int size;
|
26 |
switch(type){
|
27 |
case BOM_TYPE:
|
28 |
size = generate_bom_packet(&buf); |
29 |
wl_send_robot_to_robot_packet(DATA_REQUEST_GROUP,BOM_TYPE,buf,size,source,0);
|
30 |
case IR_TYPE:
|
31 |
size = generate_IR_packet(&buf); |
32 |
wl_send_robot_to_robot_packet(DATA_REQUEST_GROUP,IR_TYPE,buf,size,source,0);
|
33 |
case ENCODER_TYPE:
|
34 |
break;
|
35 |
//Not yet supported.
|
36 |
} |
37 |
} |
38 |
|
39 |
int generate_bom_packet(char** buf){ |
40 |
short int i=0, robot_id; |
41 |
char bom_data;
|
42 |
wl_token_iterator_begin(); |
43 |
while(wl_token_iterator_has_next() && i<=99){ |
44 |
robot_id = wl_token_iterator_next(); |
45 |
(*buf)[i++] = (char)((robot_id>>8) & 0xFF); |
46 |
(*buf)[i++] = (char)(robot_id & 0xFF); |
47 |
(*buf)[i++] = (char)(wl_token_get_my_sensor_reading(robot_id));
|
48 |
} |
49 |
//i is one greater than the index of the last byte... the size.
|
50 |
return i;
|
51 |
} |
52 |
|
53 |
int generate_IR_packet(char** buf){ |
54 |
*buf[0] = range_read_distance(IR1);
|
55 |
*buf[1] = range_read_distance(IR2);
|
56 |
*buf[2] = range_read_distance(IR3);
|
57 |
*buf[3] = range_read_distance(IR4);
|
58 |
*buf[4] = range_read_distance(IR5);
|
59 |
return 5; |
60 |
} |