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