root / branches / colonetmk2 / code / projects / libwireless / lib / wl_token_ring.c @ 1605
History | View | Annotate | Download (25.6 KB)
1 | 242 | bcoltin | /**
|
---|---|---|---|
2 | * Copyright (c) 2007 Colony Project
|
||
3 | 397 | emarinel | *
|
4 | 242 | bcoltin | * Permission is hereby granted, free of charge, to any person
|
5 | * obtaining a copy of this software and associated documentation
|
||
6 | * files (the "Software"), to deal in the Software without
|
||
7 | * restriction, including without limitation the rights to use,
|
||
8 | * copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
9 | * copies of the Software, and to permit persons to whom the
|
||
10 | * Software is furnished to do so, subject to the following
|
||
11 | * conditions:
|
||
12 | 397 | emarinel | *
|
13 | 242 | bcoltin | * The above copyright notice and this permission notice shall be
|
14 | * included in all copies or substantial portions of the Software.
|
||
15 | 397 | emarinel | *
|
16 | 242 | bcoltin | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
17 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||
18 | * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
||
19 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||
20 | * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||
21 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||
22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||
23 | * OTHER DEALINGS IN THE SOFTWARE.
|
||
24 | **/
|
||
25 | |||
26 | /**
|
||
27 | * @file wl_token_ring.c
|
||
28 | * @brief Token Ring Implementation
|
||
29 | *
|
||
30 | * Implementation of the token ring packet group.
|
||
31 | *
|
||
32 | * @author Brian Coltin, Colony Project, CMU Robotics Club
|
||
33 | **/
|
||
34 | |||
35 | 17 | bcoltin | #include <wl_token_ring.h> |
36 | |||
37 | #include <stdlib.h> |
||
38 | |||
39 | #include <wl_defs.h> |
||
40 | #include <wireless.h> |
||
41 | #include <sensor_matrix.h> |
||
42 | |||
43 | #ifdef ROBOT
|
||
44 | 86 | bcoltin | #ifndef FIREFLY
|
45 | 17 | bcoltin | #include <bom.h> |
46 | 86 | bcoltin | #endif
|
47 | 17 | bcoltin | #include <time.h> |
48 | #endif
|
||
49 | |||
50 | 717 | jknichel | //TODO: why is this in both this file and sensor_matrix.c? If it is needed in both places,
|
51 | // put it in sensor_matrix.h. This file already includes sensor_matrix.h
|
||
52 | 17 | bcoltin | #define DEFAULT_SENSOR_MATRIX_SIZE 20 |
53 | |||
54 | /*Ring States*/
|
||
55 | |||
56 | #define NONMEMBER 0 |
||
57 | #define MEMBER 1 |
||
58 | #define JOINING 2 |
||
59 | #define ACCEPTED 3 |
||
60 | #define LEAVING 4 |
||
61 | |||
62 | /*Frame Types*/
|
||
63 | 736 | bcoltin | #define TOKEN_JOIN_ACCEPT_FRAME 1 |
64 | #define WL_TOKEN_PASS_FRAME 2 |
||
65 | 17 | bcoltin | |
66 | /*Function Prototypes*/
|
||
67 | |||
68 | /*Wireless Library Prototypes*/
|
||
69 | 418 | emarinel | static void wl_token_ring_timeout_handler(void); |
70 | static void wl_token_ring_response_handler(int frame, int received); |
||
71 | static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length); |
||
72 | static void wl_token_ring_cleanup(void); |
||
73 | 17 | bcoltin | |
74 | /*Helper Functions*/
|
||
75 | 423 | emarinel | static int wl_token_pass_token(void); |
76 | 418 | emarinel | static int get_token_distance(int robot1, int robot2); |
77 | static void wl_token_get_token(void); |
||
78 | 17 | bcoltin | |
79 | /*Packet Handling Routines*/
|
||
80 | 736 | bcoltin | static void wl_token_pass_receive(int source); |
81 | static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength); |
||
82 | 418 | emarinel | static void wl_token_bom_on_receive(int source); |
83 | static void wl_token_join_receive(int source); |
||
84 | static void wl_token_join_accept_receive(int source); |
||
85 | 17 | bcoltin | |
86 | /*Global Variables*/
|
||
87 | |||
88 | //the robot we are waiting to say it has received the token. -1 if unspecified
|
||
89 | 418 | emarinel | static int wl_token_next_robot = -1; |
90 | 17 | bcoltin | |
91 | //true if the robot should be in the token ring, 0 otherwise
|
||
92 | 418 | emarinel | static int ringState = NONMEMBER; |
93 | 17 | bcoltin | //the id of the robot who accepted us into the token ring, only used in ACCEPTED state
|
94 | 418 | emarinel | static int acceptor = -1; |
95 | 17 | bcoltin | //id of the robot we are accepting
|
96 | 418 | emarinel | static int accepted = -1; |
97 | 17 | bcoltin | |
98 | //the counter for when we assume a robot is dead
|
||
99 | 418 | emarinel | static int deathDelay = -1; |
100 | 17 | bcoltin | //the counter for joining, before we form our own token ring
|
101 | 418 | emarinel | static int joinDelay = -1; |
102 | 17 | bcoltin | |
103 | 52 | bcoltin | //current robot to check in the iterator
|
104 | 418 | emarinel | static int iteratorCount = 0; |
105 | 52 | bcoltin | |
106 | 196 | bcoltin | // the amount of time a robot has had its BOM on for
|
107 | 418 | emarinel | static int bom_on_count = 0; |
108 | 196 | bcoltin | |
109 | 1456 | rcahoon | #if (!(defined ROBOT)) || (defined FIREFLY)
|
110 | 418 | emarinel | static void do_nothing(void) {} |
111 | 1456 | rcahoon | static SensorReading get_nothing(void) { SensorReading ret = { READING_UNKNOWN, READING_UNKNOWN }; return ret;} |
112 | #else
|
||
113 | static SensorReading default_get_bom(void) { int dist, dir; bom_refresh(BOM_ALL); dir = bom_get_max10(&dist); SensorReading ret = { dir, dist}; return ret; } |
||
114 | 736 | bcoltin | #endif
|
115 | 86 | bcoltin | |
116 | 17 | bcoltin | #ifdef ROBOT
|
117 | 86 | bcoltin | #ifndef FIREFLY
|
118 | 418 | emarinel | static void (*bom_on_function) (void) = bom_on; |
119 | static void (*bom_off_function) (void) = bom_off; |
||
120 | 1456 | rcahoon | static SensorReading (*get_max_bom_function) (void) = default_get_bom; |
121 | 17 | bcoltin | #else
|
122 | 418 | emarinel | static void (*bom_on_function) (void) = do_nothing; |
123 | static void (*bom_off_function) (void) = do_nothing; |
||
124 | 1456 | rcahoon | static SensorReading (*get_max_bom_function) (void) = get_nothing; |
125 | 17 | bcoltin | #endif
|
126 | 86 | bcoltin | #else
|
127 | 418 | emarinel | static void (*bom_on_function) (void) = do_nothing; |
128 | static void (*bom_off_function) (void) = do_nothing; |
||
129 | 1456 | rcahoon | static SensorReading (*get_max_bom_function) (void) = get_nothing; |
130 | 86 | bcoltin | #endif
|
131 | 17 | bcoltin | |
132 | 418 | emarinel | static PacketGroupHandler wl_token_ring_handler =
|
133 | {WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler, |
||
134 | 346 | bcoltin | wl_token_ring_response_handler, wl_token_ring_receive_handler, |
135 | wl_token_ring_cleanup}; |
||
136 | 17 | bcoltin | |
137 | /**
|
||
138 | 423 | emarinel | * Causes the robot to join an existing token ring, or create one
|
139 | * if no token ring exists. The token ring uses global and robot to robot
|
||
140 | * packets, and does not rely on any PAN.
|
||
141 | **/
|
||
142 | int wl_token_ring_join()
|
||
143 | { |
||
144 | WL_DEBUG_PRINT("Joining the token ring.\r\n");
|
||
145 | |||
146 | ringState = JOINING; |
||
147 | joinDelay = DEATH_DELAY * 2;
|
||
148 | if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN, NULL, 0, 0) != 0) { |
||
149 | return -1; |
||
150 | } |
||
151 | |||
152 | return 0; |
||
153 | } |
||
154 | |||
155 | /**
|
||
156 | * Causes the robot to leave the token ring. The robot stops
|
||
157 | * alerting others of its location, but continues storing the
|
||
158 | * locations of other robots.
|
||
159 | **/
|
||
160 | 717 | jknichel | //TODO: this function is so simple, it *may* be beneficial to inline this function. testing of if
|
161 | // it reduces code size or not should be done to be sure.
|
||
162 | 423 | emarinel | void wl_token_ring_leave()
|
163 | { |
||
164 | ringState = LEAVING; |
||
165 | } |
||
166 | |||
167 | /**
|
||
168 | 17 | bcoltin | * Initialize the token ring packet group and register it with the
|
169 | * wireless library. The robot will not join a token ring.
|
||
170 | **/
|
||
171 | 418 | emarinel | int wl_token_ring_register()
|
172 | 346 | bcoltin | { |
173 | if (wl_get_xbee_id() > 0xFF) |
||
174 | { |
||
175 | //Note: if this becomes an issue (unlikely), we could limit sensor information
|
||
176 | //to half a byte and use 12 bits for the id
|
||
177 | WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");
|
||
178 | WL_DEBUG_PRINT_INT(wl_get_xbee_id()); |
||
179 | WL_DEBUG_PRINT(".\r\n");
|
||
180 | 418 | emarinel | return -1; |
181 | 346 | bcoltin | } |
182 | 397 | emarinel | |
183 | 736 | bcoltin | sensor_matrix_create(); |
184 | 346 | bcoltin | //add ourselves to the sensor matrix
|
185 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
|
186 | 17 | bcoltin | |
187 | 346 | bcoltin | wl_register_packet_group(&wl_token_ring_handler); |
188 | 418 | emarinel | |
189 | return 0; |
||
190 | 17 | bcoltin | } |
191 | |||
192 | /**
|
||
193 | * Removes the packet group from the wireless library.
|
||
194 | **/
|
||
195 | 717 | jknichel | //TODO: this function is so simple, it *may* be beneficial to inline this function. testing of if
|
196 | // it reduces code size or not should be done to be sure.
|
||
197 | 346 | bcoltin | void wl_token_ring_unregister()
|
198 | { |
||
199 | wl_unregister_packet_group(&wl_token_ring_handler); |
||
200 | 17 | bcoltin | } |
201 | |||
202 | /**
|
||
203 | * Sets the functions that are called when the BOM ought to be
|
||
204 | 397 | emarinel | * turned on or off. This could be used for things such as
|
205 | 17 | bcoltin | * charging stations, which have multiple BOMs.
|
206 | *
|
||
207 | * @param on_function the function to be called when the BOM
|
||
208 | * should be turned on
|
||
209 | * @param off_function the function to be called when the BOM
|
||
210 | * should be turned off
|
||
211 | * @param max_bom_function the function to be called when a
|
||
212 | * measurement of the maximum BOM reading is needed.
|
||
213 | **/
|
||
214 | void wl_token_ring_set_bom_functions(void (*on_function) (void), |
||
215 | 1456 | rcahoon | void (*off_function) (void), SensorReading (*max_bom_function) (void)) |
216 | 346 | bcoltin | { |
217 | bom_on_function = on_function; |
||
218 | bom_off_function = off_function; |
||
219 | get_max_bom_function = max_bom_function; |
||
220 | 17 | bcoltin | } |
221 | |||
222 | /**
|
||
223 | * Called to cleanup the token ring packet group.
|
||
224 | **/
|
||
225 | 717 | jknichel | //TODO: this function is so simple, it *may* be beneficial to inline this function. testing of if
|
226 | // it reduces code size or not should be done to be sure.
|
||
227 | 423 | emarinel | static void wl_token_ring_cleanup() |
228 | 346 | bcoltin | { |
229 | 17 | bcoltin | } |
230 | |||
231 | /**
|
||
232 | * Called approximately every quarter second by the wireless library.
|
||
233 | **/
|
||
234 | 418 | emarinel | static void wl_token_ring_timeout_handler() |
235 | 346 | bcoltin | { |
236 | //someone is not responding, assume they are dead
|
||
237 | if (deathDelay == 0) |
||
238 | { |
||
239 | //pass the token to the next robot if we think someone has died
|
||
240 | //also, declare that person dead, as long as it isn't us
|
||
241 | if (wl_token_next_robot != wl_get_xbee_id())
|
||
242 | { |
||
243 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_token_next_robot, 0);
|
244 | 346 | bcoltin | WL_DEBUG_PRINT("Robot ");
|
245 | WL_DEBUG_PRINT_INT(wl_token_next_robot); |
||
246 | WL_DEBUG_PRINT(" has died.\r\n");
|
||
247 | wl_token_next_robot = -1;
|
||
248 | deathDelay = DEATH_DELAY; |
||
249 | } |
||
250 | 397 | emarinel | |
251 | 346 | bcoltin | // we may have been dropped from the ring when this is received
|
252 | 423 | emarinel | if (ringState == MEMBER) {
|
253 | 346 | bcoltin | wl_token_pass_token(); |
254 | 423 | emarinel | } |
255 | 346 | bcoltin | } |
256 | 17 | bcoltin | |
257 | 346 | bcoltin | //we must start our own token ring, no one is responding to us
|
258 | if (joinDelay == 0) |
||
259 | { |
||
260 | 736 | bcoltin | if (sensor_matrix_get_joined() == 0) |
261 | 346 | bcoltin | { |
262 | WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");
|
||
263 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
|
264 | 346 | bcoltin | ringState = MEMBER; |
265 | //this will make us pass the token to ourself
|
||
266 | //repeatedly, and other robots when they join
|
||
267 | deathDelay = DEATH_DELAY; |
||
268 | wl_token_next_robot = wl_get_xbee_id(); |
||
269 | } |
||
270 | else
|
||
271 | { |
||
272 | WL_DEBUG_PRINT("Attempting to join the token ring again.\r\n");
|
||
273 | //attempt to rejoin with a random delay
|
||
274 | wl_token_ring_join(); |
||
275 | joinDelay = rand() / (RAND_MAX / JOIN_DELAY) + 1;
|
||
276 | } |
||
277 | } |
||
278 | 17 | bcoltin | |
279 | 423 | emarinel | if (deathDelay >= 0) { |
280 | 346 | bcoltin | deathDelay--; |
281 | 423 | emarinel | } |
282 | |||
283 | if (joinDelay >= 0) { |
||
284 | 346 | bcoltin | joinDelay--; |
285 | 423 | emarinel | } |
286 | |||
287 | if (bom_on_count >= 0) { |
||
288 | 346 | bcoltin | bom_on_count++; |
289 | 423 | emarinel | } |
290 | 17 | bcoltin | } |
291 | |||
292 | /**
|
||
293 | * Called when the XBee tells us if a packet we sent has been received.
|
||
294 | 397 | emarinel | *
|
295 | 17 | bcoltin | * @param frame the frame number assigned when the packet was sent
|
296 | * @param received 1 if the packet was received, 0 otherwise
|
||
297 | **/
|
||
298 | 418 | emarinel | static void wl_token_ring_response_handler(int frame, int received) |
299 | 346 | bcoltin | { |
300 | if (!received)
|
||
301 | { |
||
302 | WL_DEBUG_PRINT("FAILED.\r\n");
|
||
303 | } |
||
304 | 17 | bcoltin | } |
305 | |||
306 | /**
|
||
307 | * Called when we recieve a token ring packet.
|
||
308 | * @param type the type of the packet
|
||
309 | * @param source the id of the robot who sent the packet
|
||
310 | * @param packet the data in the packet
|
||
311 | * @param length the length of the packet in bytes
|
||
312 | **/
|
||
313 | 418 | emarinel | static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length) |
314 | 346 | bcoltin | { |
315 | switch (type)
|
||
316 | { |
||
317 | case WL_TOKEN_PASS:
|
||
318 | 736 | bcoltin | wl_token_pass_receive(source); |
319 | 346 | bcoltin | break;
|
320 | 736 | bcoltin | case WL_TOKEN_SENSOR_MATRIX:
|
321 | wl_token_sensor_matrix_receive(source, packet, length); |
||
322 | break;
|
||
323 | 346 | bcoltin | case WL_TOKEN_BOM_ON:
|
324 | //add the robot to the sensor matrix if it is not already there
|
||
325 | wl_token_bom_on_receive(source); |
||
326 | break;
|
||
327 | case WL_TOKEN_JOIN:
|
||
328 | wl_token_join_receive(source); |
||
329 | break;
|
||
330 | case WL_TOKEN_JOIN_ACCEPT:
|
||
331 | wl_token_join_accept_receive(source); |
||
332 | break;
|
||
333 | default:
|
||
334 | WL_DEBUG_PRINT("Unimplemented token ring packet received.\r\n");
|
||
335 | break;
|
||
336 | } |
||
337 | 17 | bcoltin | } |
338 | |||
339 | /**
|
||
340 | * Returns the BOM reading robot source has for robot dest.
|
||
341 | *
|
||
342 | * @param source the robot that made the BOM reading
|
||
343 | * @param dest the robot whose relative location is returned
|
||
344 | *
|
||
345 | * @return a BOM reading from robot source to robot dest,
|
||
346 | * in the range 0-15, or -1 if it is unknown
|
||
347 | **/
|
||
348 | 1456 | rcahoon | SensorReading * wl_token_get_sensor_reading(int source, int dest) |
349 | 346 | bcoltin | { |
350 | if (wl_token_is_robot_in_ring(dest) &&
|
||
351 | 418 | emarinel | (source == wl_get_xbee_id() || wl_token_is_robot_in_ring(source))) { |
352 | 736 | bcoltin | return sensor_matrix_get_reading(source, dest);
|
353 | 418 | emarinel | } |
354 | |||
355 | 1456 | rcahoon | return NULL; |
356 | 17 | bcoltin | } |
357 | |||
358 | /**
|
||
359 | * Returns the BOM reading we have for robot dest.
|
||
360 | 397 | emarinel | *
|
361 | 17 | bcoltin | * @param dest the robot whose relative location is returned
|
362 | *
|
||
363 | * @return a BOM reading from us to robot dest, in the range
|
||
364 | * 0-15, or -1 if it is unkown
|
||
365 | **/
|
||
366 | 717 | jknichel | //TODO: this function is so simple, it *may* be beneficial to inline this function. testing of if
|
367 | // it reduces code size or not should be done to be sure.
|
||
368 | 1456 | rcahoon | SensorReading * wl_token_get_my_sensor_reading(int dest)
|
369 | 346 | bcoltin | { |
370 | return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
|
||
371 | 17 | bcoltin | } |
372 | |||
373 | 423 | emarinel | |
374 | 17 | bcoltin | /**
|
375 | 423 | emarinel | * Returns the number of robots in the token ring.
|
376 | *
|
||
377 | * @return the number of robots in the token ring
|
||
378 | **/
|
||
379 | 717 | jknichel | //TODO: this function is so simple, it *may* be beneficial to inline this function. testing of if
|
380 | // it reduces code size or not should be done to be sure.
|
||
381 | 423 | emarinel | int wl_token_get_robots_in_ring(void) |
382 | { |
||
383 | 736 | bcoltin | return sensor_matrix_get_joined();
|
384 | 423 | emarinel | } |
385 | |||
386 | /**
|
||
387 | * Returns true if the specified robot is in the token ring, false
|
||
388 | * otherwise.
|
||
389 | *
|
||
390 | * @param robot the robot to check for whether it is in the token ring
|
||
391 | * @return nonzero if the robot is in the token ring, zero otherwise
|
||
392 | **/
|
||
393 | 717 | jknichel | //TODO: this function is so simple, it *may* be beneficial to inline this function. testing of if
|
394 | // it reduces code size or not should be done to be sure.
|
||
395 | 423 | emarinel | int wl_token_is_robot_in_ring(int robot) |
396 | { |
||
397 | 736 | bcoltin | return sensor_matrix_get_in_ring(robot);
|
398 | 423 | emarinel | } |
399 | |||
400 | /**
|
||
401 | * Begins iterating through the robots in the token ring.
|
||
402 | *
|
||
403 | * @see wl_token_iterator_has_next, wl_token_iterator_next
|
||
404 | **/
|
||
405 | void wl_token_iterator_begin(void) |
||
406 | { |
||
407 | int i = 0; |
||
408 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
409 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
410 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
411 | // the value in a variable and check against that variable in the loop condition
|
||
412 | 736 | bcoltin | while (!sensor_matrix_get_in_ring(i) && i < sensor_matrix_get_size()) {
|
413 | 423 | emarinel | i++; |
414 | } |
||
415 | |||
416 | 717 | jknichel | //TODO: if you do the above comment, then you can compare this to the variable also
|
417 | 736 | bcoltin | if (i == sensor_matrix_get_size()) {
|
418 | 423 | emarinel | i = -1;
|
419 | } |
||
420 | |||
421 | iteratorCount = i; |
||
422 | } |
||
423 | |||
424 | /**
|
||
425 | * Returns true if there are more robots in the token ring
|
||
426 | * to iterate through, and false otherwise.
|
||
427 | *
|
||
428 | * @return nonzero if there are more robots to iterate through,
|
||
429 | * zero otherwise
|
||
430 | *
|
||
431 | * @see wl_token_iterator_begin, wl_token_iterator_next
|
||
432 | **/
|
||
433 | 717 | jknichel | //TODO: this function is so simple, it *may* be beneficial to inline this function. testing of if
|
434 | // it reduces code size or not should be done to be sure.
|
||
435 | 423 | emarinel | int wl_token_iterator_has_next(void) |
436 | { |
||
437 | return iteratorCount != -1; |
||
438 | } |
||
439 | |||
440 | /**
|
||
441 | * Returns the next robot ID in the token ring.
|
||
442 | *
|
||
443 | * @return the next robot ID in the token ring, or -1 if none exists
|
||
444 | *
|
||
445 | * @see wl_token_iterator_begin, wl_token_iterator_has_next
|
||
446 | **/
|
||
447 | int wl_token_iterator_next(void) |
||
448 | { |
||
449 | int result = iteratorCount;
|
||
450 | if (result < 0) { |
||
451 | return result;
|
||
452 | } |
||
453 | |||
454 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
455 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
456 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
457 | // the value in a variable and check against that variable in the loop condition
|
||
458 | 423 | emarinel | iteratorCount++; |
459 | 736 | bcoltin | while (!sensor_matrix_get_in_ring(iteratorCount)
|
460 | && iteratorCount < sensor_matrix_get_size()) { |
||
461 | 423 | emarinel | iteratorCount++; |
462 | } |
||
463 | |||
464 | 717 | jknichel | //TODO: if you do the above comment, then you can compare this to the variable also
|
465 | 736 | bcoltin | if (iteratorCount == sensor_matrix_get_size()) {
|
466 | 423 | emarinel | iteratorCount = -1;
|
467 | } |
||
468 | |||
469 | return result;
|
||
470 | } |
||
471 | |||
472 | /**
|
||
473 | * Returns the number of robots currently in the token ring.
|
||
474 | *
|
||
475 | * @return the number of robots in the token ring
|
||
476 | **/
|
||
477 | 717 | jknichel | //TODO: this function is so simple, it *may* be beneficial to inline this function. testing of if
|
478 | // it reduces code size or not should be done to be sure.
|
||
479 | 423 | emarinel | int wl_token_get_num_robots(void) |
480 | { |
||
481 | 736 | bcoltin | return sensor_matrix_get_joined();
|
482 | 423 | emarinel | } |
483 | |||
484 | /**
|
||
485 | * Returns the number of robots in the sensor matrix.
|
||
486 | *
|
||
487 | * @return the number of robots in the sensor matrix
|
||
488 | **/
|
||
489 | 717 | jknichel | //TODO: this function is so simple, it *may* be beneficial to inline this function. testing of if
|
490 | // it reduces code size or not should be done to be sure.
|
||
491 | 423 | emarinel | int wl_token_get_matrix_size(void) |
492 | { |
||
493 | 736 | bcoltin | return sensor_matrix_get_size();
|
494 | 423 | emarinel | } |
495 | |||
496 | /**
|
||
497 | 17 | bcoltin | * This method is called when we receive a token pass packet.
|
498 | 736 | bcoltin | *
|
499 | * @param source the robot who passed the token to us.
|
||
500 | **/
|
||
501 | static void wl_token_pass_receive(int source) |
||
502 | { |
||
503 | WL_DEBUG_PRINT("Received token from ");
|
||
504 | WL_DEBUG_PRINT_INT(source); |
||
505 | WL_DEBUG_PRINT(", expected ");
|
||
506 | WL_DEBUG_PRINT_INT(wl_token_next_robot); |
||
507 | WL_DEBUG_PRINT(".\n");
|
||
508 | // this prevents two tokens from being passed around at a time (second clause is in case we are joining)
|
||
509 | if ((source != wl_token_next_robot && wl_get_xbee_id() != wl_token_next_robot) && bom_on_count <= DEATH_DELAY / 2 && |
||
510 | ringState != ACCEPTED) |
||
511 | { |
||
512 | WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");
|
||
513 | WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");
|
||
514 | return;
|
||
515 | } |
||
516 | bom_on_count = -1;
|
||
517 | deathDelay = -1;
|
||
518 | sensor_matrix_set_in_ring(source, 1);
|
||
519 | wl_token_get_token(); |
||
520 | } |
||
521 | |||
522 | /**
|
||
523 | * This method is called when we receive a token pass packet.
|
||
524 | 17 | bcoltin | * @param source is the robot it came from
|
525 | * @param nextRobot is the robot the token was passed to
|
||
526 | * @param sensorData a char with an id followed by a char with the sensor
|
||
527 | * reading for that robot, repeated for sensorDataLength bytes
|
||
528 | * @param sensorDataLength the length in bytes of sensorData
|
||
529 | */
|
||
530 | 736 | bcoltin | static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength) |
531 | 346 | bcoltin | { |
532 | int i, j;
|
||
533 | 736 | bcoltin | char nextRobot;
|
534 | 1456 | rcahoon | SensorReading r; |
535 | 196 | bcoltin | |
536 | 346 | bcoltin | bom_on_count = -1;
|
537 | deathDelay = -1;
|
||
538 | 736 | bcoltin | sensor_matrix_set_in_ring(source, 1);
|
539 | 17 | bcoltin | |
540 | 346 | bcoltin | //with this packet, we are passed the id of the next robot in the ring
|
541 | //and the sensor matrix, a list of id and sensor reading pairs (two bytes for both)
|
||
542 | j = 0;
|
||
543 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
544 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
545 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
546 | // the value in a variable and check against that variable in the loop condition
|
||
547 | 736 | bcoltin | for (i = 0; i < sensor_matrix_get_size(); i++) |
548 | 346 | bcoltin | { |
549 | 423 | emarinel | if (i == source) {
|
550 | 346 | bcoltin | continue;
|
551 | 423 | emarinel | } |
552 | 397 | emarinel | |
553 | 346 | bcoltin | //set the sensor information we receive
|
554 | 1456 | rcahoon | if (j < sensorDataLength / 2 && sensorData[3 * j] == i) |
555 | 346 | bcoltin | { |
556 | //the robot we were going to accept has already been accepted
|
||
557 | if (accepted == i)
|
||
558 | { |
||
559 | accepted = -1;
|
||
560 | WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");
|
||
561 | } |
||
562 | 1456 | rcahoon | r.dir = sensorData[3 * j + 1]; |
563 | r.dist = sensorData[3 * j + 2]; |
||
564 | sensor_matrix_set_reading(source, i, r); |
||
565 | 736 | bcoltin | if (!sensor_matrix_get_in_ring(i))
|
566 | { |
||
567 | WL_DEBUG_PRINT("Robot ");
|
||
568 | WL_DEBUG_PRINT_INT(i); |
||
569 | WL_DEBUG_PRINT(" has been added to the sensor matrix of robot ");
|
||
570 | WL_DEBUG_PRINT_INT(wl_get_xbee_id()); |
||
571 | WL_DEBUG_PRINT(" due to a packet from robot ");
|
||
572 | WL_DEBUG_PRINT_INT(source); |
||
573 | WL_DEBUG_PRINT(".\r\n");
|
||
574 | } |
||
575 | sensor_matrix_set_in_ring(i, 1);
|
||
576 | 346 | bcoltin | j++; |
577 | } |
||
578 | else
|
||
579 | { |
||
580 | 736 | bcoltin | if (sensor_matrix_get_in_ring(i))
|
581 | 346 | bcoltin | { |
582 | WL_DEBUG_PRINT("Robot ");
|
||
583 | WL_DEBUG_PRINT_INT(i); |
||
584 | WL_DEBUG_PRINT(" has been removed from the sensor matrix of robot ");
|
||
585 | WL_DEBUG_PRINT_INT(wl_get_xbee_id()); |
||
586 | WL_DEBUG_PRINT(" due to a packet from robot ");
|
||
587 | WL_DEBUG_PRINT_INT(source); |
||
588 | WL_DEBUG_PRINT(".\r\n");
|
||
589 | 736 | bcoltin | sensor_matrix_set_in_ring(i, 0);
|
590 | 346 | bcoltin | } |
591 | 17 | bcoltin | |
592 | 346 | bcoltin | if (i == wl_get_xbee_id() && ringState == MEMBER)
|
593 | { |
||
594 | ringState = NONMEMBER; |
||
595 | wl_token_ring_join(); |
||
596 | 397 | emarinel | |
597 | 346 | bcoltin | WL_DEBUG_PRINT("We have been removed from the ring ");
|
598 | WL_DEBUG_PRINT("and are rejoining.\r\n");
|
||
599 | } |
||
600 | 397 | emarinel | |
601 | 346 | bcoltin | //the person who accepted us is dead... let's ask again
|
602 | if (i == acceptor)
|
||
603 | { |
||
604 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
|
605 | 346 | bcoltin | ringState = NONMEMBER; |
606 | acceptor = -1;
|
||
607 | wl_token_ring_join(); |
||
608 | } |
||
609 | } |
||
610 | } |
||
611 | 736 | bcoltin | |
612 | // get the next robot in the token ring
|
||
613 | i = source + 1;
|
||
614 | while (1) |
||
615 | { |
||
616 | if (i == sensor_matrix_get_size()) {
|
||
617 | i = 0;
|
||
618 | } |
||
619 | 17 | bcoltin | |
620 | 736 | bcoltin | if (sensor_matrix_get_in_ring(i) || i == source)
|
621 | { |
||
622 | nextRobot = (char)i;
|
||
623 | break;
|
||
624 | } |
||
625 | 397 | emarinel | |
626 | 736 | bcoltin | i++; |
627 | } |
||
628 | |||
629 | if (nextRobot != wl_get_xbee_id())
|
||
630 | wl_token_next_robot = nextRobot; |
||
631 | |||
632 | 346 | bcoltin | deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY; |
633 | 397 | emarinel | |
634 | 736 | bcoltin | if (sensor_matrix_get_joined() == 0 && ringState == JOINING) |
635 | wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME); |
||
636 | 17 | bcoltin | } |
637 | |||
638 | /**
|
||
639 | * Gets the distance in the token ring between two robots.
|
||
640 | *
|
||
641 | * @param robot1 the first robot
|
||
642 | * @param robot2 the second robot
|
||
643 | *
|
||
644 | * @return the number of passes before the token is expected
|
||
645 | * to reach robot2 from robot1
|
||
646 | **/
|
||
647 | 423 | emarinel | static int get_token_distance(int robot1, int robot2) |
648 | 346 | bcoltin | { |
649 | int curr = robot1 + 1; |
||
650 | int count = 1; |
||
651 | while (1) |
||
652 | { |
||
653 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
654 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
655 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
656 | // the value in a variable and check against that variable in the loop condition
|
||
657 | 736 | bcoltin | if (curr == sensor_matrix_get_size())
|
658 | 346 | bcoltin | curr = 0;
|
659 | if (curr == robot2)
|
||
660 | break;
|
||
661 | 736 | bcoltin | if (sensor_matrix_get_in_ring(curr))
|
662 | 346 | bcoltin | count++; |
663 | curr++; |
||
664 | } |
||
665 | return count;
|
||
666 | 17 | bcoltin | } |
667 | |||
668 | /**
|
||
669 | * Passes the token to the next robot in the token ring.
|
||
670 | **/
|
||
671 | 423 | emarinel | static int wl_token_pass_token() |
672 | 346 | bcoltin | { |
673 | 736 | bcoltin | char nextRobot = 0xFF; |
674 | 346 | bcoltin | int i = wl_get_xbee_id() + 1; |
675 | 1456 | rcahoon | char buf[3 * sensor_matrix_get_size()]; |
676 | 346 | bcoltin | if (accepted == -1) |
677 | { |
||
678 | while (1) |
||
679 | { |
||
680 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
681 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
682 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
683 | // the value in a variable and check against that variable in the loop condition
|
||
684 | 736 | bcoltin | if (i == sensor_matrix_get_size()) {
|
685 | 346 | bcoltin | i = 0;
|
686 | 423 | emarinel | } |
687 | |||
688 | 736 | bcoltin | if (sensor_matrix_get_in_ring(i))
|
689 | 346 | bcoltin | { |
690 | nextRobot = (char)i;
|
||
691 | break;
|
||
692 | } |
||
693 | 423 | emarinel | |
694 | 346 | bcoltin | i++; |
695 | } |
||
696 | } |
||
697 | else
|
||
698 | { |
||
699 | WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");
|
||
700 | //add a new robot to the token ring
|
||
701 | 736 | bcoltin | sensor_matrix_set_in_ring(accepted, 1);
|
702 | 346 | bcoltin | nextRobot = accepted; |
703 | accepted = -1;
|
||
704 | } |
||
705 | 17 | bcoltin | |
706 | 346 | bcoltin | int j = 0; |
707 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
708 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
709 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
710 | // the value in a variable and check against that variable in the loop condition
|
||
711 | 736 | bcoltin | for (i = 0; i < sensor_matrix_get_size(); i++) { |
712 | if (sensor_matrix_get_in_ring(i) && i != wl_get_xbee_id())
|
||
713 | 346 | bcoltin | { |
714 | 1456 | rcahoon | SensorReading *r = sensor_matrix_get_reading(wl_get_xbee_id(), i); |
715 | buf[3*j] = i;
|
||
716 | buf[3*j + 1] = r->dir; |
||
717 | buf[3*j + 2] = r->dist; |
||
718 | 346 | bcoltin | j++; |
719 | } |
||
720 | 423 | emarinel | } |
721 | 397 | emarinel | |
722 | 1456 | rcahoon | int packetSize = 3 * j * sizeof(char); |
723 | 346 | bcoltin | WL_DEBUG_PRINT("Passing the token to robot ");
|
724 | 736 | bcoltin | WL_DEBUG_PRINT_INT(nextRobot); |
725 | 346 | bcoltin | WL_DEBUG_PRINT(".\r\n");
|
726 | 736 | bcoltin | if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0) |
727 | 423 | emarinel | return -1; |
728 | 736 | bcoltin | if (wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME)) |
729 | return -1; |
||
730 | 17 | bcoltin | |
731 | 346 | bcoltin | wl_token_next_robot = nextRobot; |
732 | deathDelay = DEATH_DELAY; |
||
733 | 423 | emarinel | |
734 | return 0; |
||
735 | 17 | bcoltin | } |
736 | |||
737 | /**
|
||
738 | * Called when a packet is received stating that another robot has turned
|
||
739 | * its BOM on. Our BOM is then read, and the data is added to the sensor
|
||
740 | * matrix.
|
||
741 | *
|
||
742 | * @param source the robot whose BOM is on
|
||
743 | **/
|
||
744 | 423 | emarinel | static void wl_token_bom_on_receive(int source) |
745 | 346 | bcoltin | { |
746 | 1456 | rcahoon | SensorReading r; |
747 | 1443 | rcahoon | |
748 | 1605 | rcahoon | int i, j, max = -1; |
749 | int vals[16]; |
||
750 | |||
751 | 346 | bcoltin | WL_DEBUG_PRINT("Robot ");
|
752 | WL_DEBUG_PRINT_INT(source); |
||
753 | WL_DEBUG_PRINT(" has flashed its bom.\r\n");
|
||
754 | 397 | emarinel | |
755 | 346 | bcoltin | bom_on_count = 0;
|
756 | 138 | bcoltin | |
757 | 1456 | rcahoon | r = get_max_bom_function(); |
758 | 1605 | rcahoon | |
759 | /* Record values into an array */
|
||
760 | for (i = 0; i < 16; i++) { |
||
761 | vals[i] = bom_get(i); |
||
762 | if (max < vals[i])
|
||
763 | max = vals[i]; |
||
764 | } |
||
765 | |||
766 | /* Display results */
|
||
767 | for (i = 0; i < 16; i++) { |
||
768 | |||
769 | usb_puti(vals[i]); |
||
770 | usb_putc('\t');
|
||
771 | |||
772 | for (j = 0; j < (int)((max - vals[i]) / 5); j++) { |
||
773 | usb_putc('#');
|
||
774 | } |
||
775 | |||
776 | usb_putc('\n');
|
||
777 | |||
778 | } |
||
779 | |||
780 | |||
781 | |||
782 | 736 | bcoltin | sensor_matrix_set_reading(wl_get_xbee_id(), |
783 | 1456 | rcahoon | source, r); |
784 | 1443 | rcahoon | |
785 | WL_DEBUG_PRINT("Max: ");
|
||
786 | 1456 | rcahoon | WL_DEBUG_PRINT_INT(r.dir); |
787 | WL_DEBUG_PRINT("\tDist: ");
|
||
788 | WL_DEBUG_PRINT_INT(r.dist); |
||
789 | 1443 | rcahoon | WL_DEBUG_PRINT("\n\n");
|
790 | 17 | bcoltin | } |
791 | |||
792 | /**
|
||
793 | * This method is called when we receive the token. Upon receiving
|
||
794 | * the token, we must send a BOM_ON packet, flash the BOM, and send
|
||
795 | * the token to the next robot.
|
||
796 | 397 | emarinel | *
|
797 | 17 | bcoltin | * If there is a pending request for the token, this is processed first.
|
798 | **/
|
||
799 | 423 | emarinel | static void wl_token_get_token() |
800 | 346 | bcoltin | { |
801 | WL_DEBUG_PRINT("We have the token.\r\n");
|
||
802 | if (ringState == ACCEPTED)
|
||
803 | { |
||
804 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
|
805 | 346 | bcoltin | WL_DEBUG_PRINT("Now a member of the token ring.\r\n");
|
806 | ringState = MEMBER; |
||
807 | joinDelay = -1;
|
||
808 | } |
||
809 | 17 | bcoltin | |
810 | 346 | bcoltin | if (ringState == LEAVING || ringState == NONMEMBER)
|
811 | { |
||
812 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
|
813 | 346 | bcoltin | if (ringState == NONMEMBER)
|
814 | { |
||
815 | WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
|
||
816 | } |
||
817 | return;
|
||
818 | } |
||
819 | 397 | emarinel | |
820 | 346 | bcoltin | WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");
|
821 | 423 | emarinel | wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON, NULL, 0, 0); |
822 | 17 | bcoltin | |
823 | 346 | bcoltin | bom_on_function(); |
824 | #ifdef ROBOT
|
||
825 | delay_ms(BOM_DELAY); |
||
826 | #endif
|
||
827 | bom_off_function(); |
||
828 | 397 | emarinel | |
829 | 736 | bcoltin | if (!sensor_matrix_get_in_ring(wl_get_xbee_id()))
|
830 | 346 | bcoltin | { |
831 | WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
|
||
832 | return;
|
||
833 | } |
||
834 | 397 | emarinel | |
835 | 346 | bcoltin | wl_token_pass_token(); |
836 | 17 | bcoltin | } |
837 | |||
838 | /**
|
||
839 | * Called when a request to join the token ring is received.
|
||
840 | * If we are the robot preceding the requester in the ring,
|
||
841 | * we respond with a JOIN_ACCEPT packet and pass the token to
|
||
842 | * this robot when we receive the token.
|
||
843 | *
|
||
844 | * @param source the robot who requested to join
|
||
845 | **/
|
||
846 | 423 | emarinel | static void wl_token_join_receive(int source) |
847 | 346 | bcoltin | { |
848 | WL_DEBUG_PRINT("Received joining request from robot ");
|
||
849 | WL_DEBUG_PRINT_INT(source); |
||
850 | WL_DEBUG_PRINT(".\r\n");
|
||
851 | 17 | bcoltin | |
852 | 346 | bcoltin | //we cannot accept the request if we are not a member
|
853 | if (ringState != MEMBER)
|
||
854 | return;
|
||
855 | //if they didn't get our response, see if we should respond again
|
||
856 | if (accepted == source)
|
||
857 | accepted = -1;
|
||
858 | //we can only accept one request at a time
|
||
859 | if (accepted != -1) |
||
860 | return;
|
||
861 | 397 | emarinel | |
862 | 346 | bcoltin | //check if we are the preceding robot in the token ring
|
863 | int i = source - 1; |
||
864 | while (1) |
||
865 | { |
||
866 | if (i < 0) |
||
867 | 736 | bcoltin | i = sensor_matrix_get_size() - 1;
|
868 | 717 | jknichel | |
869 | 346 | bcoltin | //we must send a join acceptance
|
870 | if (i == wl_get_xbee_id())
|
||
871 | break;
|
||
872 | 17 | bcoltin | |
873 | 346 | bcoltin | //another robot will handle it
|
874 | 736 | bcoltin | if (sensor_matrix_get_in_ring(i))
|
875 | 346 | bcoltin | return;
|
876 | 717 | jknichel | |
877 | 346 | bcoltin | i--; |
878 | } |
||
879 | 17 | bcoltin | |
880 | 346 | bcoltin | accepted = source; |
881 | wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT, |
||
882 | NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME); |
||
883 | 397 | emarinel | |
884 | 346 | bcoltin | WL_DEBUG_PRINT("Accepting robot ");
|
885 | WL_DEBUG_PRINT_INT(source); |
||
886 | WL_DEBUG_PRINT(" into the token ring.\r\n");
|
||
887 | 17 | bcoltin | |
888 | 346 | bcoltin | // the token ring has not started yet
|
889 | 736 | bcoltin | if (sensor_matrix_get_joined() == 1) |
890 | 346 | bcoltin | wl_token_pass_token(); |
891 | 17 | bcoltin | } |
892 | |||
893 | /**
|
||
894 | * Called when we receive a JOIN_ACCEPT packet in attempting to join
|
||
895 | * the token ring.
|
||
896 | * Our attempt to join the ring is stopped, and we wait for the token.
|
||
897 | *
|
||
898 | * @param source the robot who accepted us
|
||
899 | **/
|
||
900 | 423 | emarinel | static void wl_token_join_accept_receive(int source) |
901 | 346 | bcoltin | { |
902 | WL_DEBUG_PRINT("Accepted into the token ring by robot ");
|
||
903 | WL_DEBUG_PRINT_INT(source); |
||
904 | WL_DEBUG_PRINT(".\r\n");
|
||
905 | joinDelay = JOIN_DELAY; |
||
906 | ringState = ACCEPTED; |
||
907 | acceptor = source; |
||
908 | 17 | bcoltin | |
909 | 346 | bcoltin | //add ourselves to the token ring
|
910 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
|
911 | 17 | bcoltin | } |