Revision 124
More work. Eugene better use this.
branches/slam/code/projects/colonet/DataRequests/robot/data_response.c | ||
---|---|---|
1 |
#include "data_response.h" |
|
2 |
#include <dragonfly_lib.h> |
|
3 |
#include "wl_token_ring.h" |
|
4 |
#include "wireless.h" |
|
1 | 5 |
|
2 |
void handle_bom_request(void){ |
|
3 | 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; |
|
4 | 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 |
} |
branches/slam/code/projects/colonet/DataRequests/robot/data_response.h | ||
---|---|---|
1 |
#ifndef _DATA_RESPONSE_H |
|
2 |
#define _DATA_REQUESTS_H |
|
1 | 3 |
|
4 |
#define DATA_REQUEST_GROUP 23 |
|
5 |
#define BOM_TYPE 0 |
|
6 |
#define IR_TYPE 1 |
|
7 |
#define ENCODER_TYPE 2 |
|
2 | 8 |
|
9 |
#define MAX_PACKET_LENGTH 100 |
|
10 |
|
|
11 |
/* This requires the wireless library |
|
12 |
and the token ring to already be initialized.*/ |
|
3 | 13 |
void data_request_init(void); |
14 |
|
|
15 |
#endif |
Also available in: Unified diff