root / trunk / code / projects / slam / robot_main.c @ 71
History | View | Annotate | Download (3.13 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wireless.h> |
3 |
#include <wl_token_ring.h> |
4 |
#include <smart_run_around_fsm.h> |
5 |
|
6 |
#define BOM_POST_DELAY 50 |
7 |
#define SLAM_PACKET_GROUP 11 |
8 |
#define CONTROL_PACKET_TYPE 'C' |
9 |
#define REQUEST_PACKET_TYPE 'R' |
10 |
#define ROBOT_RESPONSE 'I' |
11 |
|
12 |
//REQUEST_PACKET_ORGANIZATION
|
13 |
#define REQUEST_PACKET_SIZE 50 |
14 |
#define IR_1_LOC 0 |
15 |
#define IR_2_LOC 1 |
16 |
#define IR_3_LOC 2 |
17 |
#define IR_4_LOC 3 |
18 |
#define IR_5_LOC 4 |
19 |
#define BOM_START_LOC 5 |
20 |
|
21 |
void bom_pre(void); |
22 |
void bom_post(void); |
23 |
|
24 |
//Packet Handler Functions
|
25 |
void timeout_handle(void); |
26 |
void response_handle(int frame, int received); |
27 |
void receive_handle(char type, int source, unsigned char* packet, int length); |
28 |
void unregister(void); |
29 |
|
30 |
int make_request_packet(char* packet); |
31 |
|
32 |
int IR[5]; |
33 |
|
34 |
int main(void) |
35 |
{ |
36 |
|
37 |
dragonfly_init(ALL_ON); |
38 |
|
39 |
wl_init(); |
40 |
|
41 |
wl_token_ring_register(); |
42 |
wl_token_ring_join(); |
43 |
wl_token_ring_set_bom_functions(bom_pre, bom_post, get_max_bom); |
44 |
|
45 |
PacketGroupHandler packet_group_handler; |
46 |
packet_group_handler.groupCode=SLAM_PACKET_GROUP; |
47 |
packet_group_handler.timeout_handler=timeout_handle; |
48 |
packet_group_handler.handle_response=response_handle; |
49 |
packet_group_handler.handle_receive=receive_handle; |
50 |
packet_group_handler.unregister=unregister; |
51 |
|
52 |
wl_register_packet_group(&packet_group_handler); |
53 |
|
54 |
int i;
|
55 |
while(1){ |
56 |
wl_do(); |
57 |
run_around_FSM(); |
58 |
for(i=0;i<5;i++){ |
59 |
IR[i]=range_read_distance(i); |
60 |
} |
61 |
} |
62 |
|
63 |
wl_unregister_packet_group(&packet_group_handler); |
64 |
} |
65 |
|
66 |
void bom_pre(void){ |
67 |
bom_on(); |
68 |
} |
69 |
|
70 |
void bom_post(void){ |
71 |
delay_ms(BOM_POST_DELAY); |
72 |
bom_off(); |
73 |
} |
74 |
|
75 |
//These can probably be NULL..
|
76 |
|
77 |
void timeout_handle(void){ |
78 |
|
79 |
} |
80 |
|
81 |
void response_handle(int frame, int received){ |
82 |
|
83 |
} |
84 |
|
85 |
void receive_handle(char type, int source, unsigned char* packet, int length){ |
86 |
int i;
|
87 |
if(type==REQUEST_PACKET_TYPE){
|
88 |
//Send a message back to our computer overlord.
|
89 |
char buf[PACKET_BUFFER_SIZE];
|
90 |
int len = make_request_packet(&buf);
|
91 |
char packet[len];
|
92 |
for(i=0; i< len ; i++){ |
93 |
packet[i]=buf[i]; |
94 |
} |
95 |
|
96 |
wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP, ROBOT_RESPONSE, packet, len, source , 0);
|
97 |
} |
98 |
else if(type == CONTROL_PACKET_TYPE){ |
99 |
for(i=0;i<length;i++){ |
100 |
|
101 |
} |
102 |
} |
103 |
} |
104 |
|
105 |
|
106 |
/**
|
107 |
|
108 |
Packets are structured {IR1, IR2, IR3, IR4, IR5,
|
109 |
BomID1, ... , BomValue1, BomID2, ... , BomValue2, etc.}
|
110 |
Total size is 5 + 3*numRobots. So Xbee should support
|
111 |
up to 31 robots before we need to restructure the
|
112 |
communication between the computer and the robot.
|
113 |
|
114 |
*/
|
115 |
int make_request_packet(char** packet){ |
116 |
int i,j;
|
117 |
for(i=0;i<5;i++){ |
118 |
(*packet)[i] = IR[i]; |
119 |
} |
120 |
i=5;
|
121 |
wl_token_iterator_begin(); |
122 |
int id = wl_get_xbee_id();
|
123 |
|
124 |
while(wl_token_iterator_has_next()){
|
125 |
int robot_in_view = wl_token_iterator_next();
|
126 |
|
127 |
//Robot int's are two bytes.
|
128 |
(*packet)[i] = robot_in_view; |
129 |
i+=2;
|
130 |
|
131 |
int bom_number = wl_token_get_sensor_reading(id,robot_in_view);
|
132 |
//Bom reading of 255 denotes an unseen node.
|
133 |
if(bom_number<0) bom_number = 255; |
134 |
bom_number %= 256;
|
135 |
char bom_value = (char) bom_number; |
136 |
(*packet)[i] = bom_value; |
137 |
i++; |
138 |
} |
139 |
//Size is one bom byte towards every robot, and 5 IR bytes.
|
140 |
return wl_token_get_num_robots()+5; |
141 |
} |
142 |
|