root / branches / autonomous_recharging / code / projects / autonomous_recharging / dragonfly / wl_recharge_group.c @ 116
History | View | Annotate | Download (11 KB)
1 | 93 | jykong | #include "wl_recharge_group.h" |
---|---|---|---|
2 | |||
3 | #include <wireless.h> |
||
4 | #include <wl_defs.h> |
||
5 | |||
6 | 116 | bcoltin | #include <serial.h> |
7 | 93 | jykong | |
8 | 116 | bcoltin | #include "recharge_defs.h" |
9 | |||
10 | 93 | jykong | // Function prototypes
|
11 | void wl_recharge_timeout_handler(void); |
||
12 | void wl_recharge_response_handler(int frame, int received); |
||
13 | void wl_recharge_handle_receive(char type, int source, unsigned char* packet, |
||
14 | int length);
|
||
15 | void wl_recharge_cleanup(void); |
||
16 | |||
17 | // messgae handlers
|
||
18 | 105 | bcoltin | void wl_recharge_available_station(int source); |
19 | 93 | jykong | void wl_recharge_request_accepted(int source); |
20 | void wl_recharge_verify(int source); |
||
21 | void wl_recharge_cancel(int source); |
||
22 | void wl_recharge_seeking(int source); |
||
23 | void wl_recharge_docked(int source); |
||
24 | |||
25 | // sending packets
|
||
26 | void wl_recharge_send_poll(void); |
||
27 | void wl_recharge_send_request(int dest); |
||
28 | void wl_recharge_send_verify(int dest); |
||
29 | void wl_recharge_send_cancel(int dest); |
||
30 | void wl_recharge_send_seeking(int dest); |
||
31 | void wl_recharge_send_docked(int dest); |
||
32 | |||
33 | // the handler
|
||
34 | PacketGroupHandler wl_recharge_handler = |
||
35 | {WL_RECHARGE_GROUP, wl_recharge_timeout_handler, |
||
36 | wl_recharge_response_handler, wl_recharge_handle_receive, |
||
37 | wl_recharge_cleanup}; |
||
38 | |||
39 | //timing
|
||
40 | //timer ticks between polls
|
||
41 | #define POLL_DELAY 4 |
||
42 | #define REQUEST_DELAY 6 |
||
43 | #define VERIFY_DELAY 8 |
||
44 | #define VERIFY_FREQUENCY 4 |
||
45 | |||
46 | //frames
|
||
47 | #define REQUEST_FRAME 1 |
||
48 | #define VERIFY_FRAME 2 |
||
49 | #define CANCEL_FRAME 3 |
||
50 | #define SEEKING_FRAME 4 |
||
51 | #define DOCKED_FRAME 5 |
||
52 | |||
53 | //global varaibles
|
||
54 | //state variables
|
||
55 | 105 | bcoltin | int recharge_state = NOT_RECHARGING;
|
56 | 93 | jykong | int station = -1; |
57 | |||
58 | //counter variables
|
||
59 | int pollCount = 0; |
||
60 | int requestCount = 0; |
||
61 | int verifyCount = 0; |
||
62 | |||
63 | 116 | bcoltin | |
64 | 93 | jykong | /**
|
65 | * Register this packet group with the wireless library.
|
||
66 | * This function must be called before any other wl_recharge
|
||
67 | * function. Also, wl_token_ring_register must be called
|
||
68 | * before this function.
|
||
69 | *
|
||
70 | * @see wl_recharge_unregister
|
||
71 | **/
|
||
72 | void wl_recharge_register(void) |
||
73 | { |
||
74 | wl_register_packet_group(&wl_recharge_handler); |
||
75 | } |
||
76 | |||
77 | /**
|
||
78 | * Called to unregister this packet group with the wireless
|
||
79 | * library.
|
||
80 | *
|
||
81 | * @see wl_recharge_register
|
||
82 | **/
|
||
83 | void wl_recharge_unregister(void) |
||
84 | { |
||
85 | wl_unregister_packet_group(&wl_recharge_handler); |
||
86 | } |
||
87 | |||
88 | /**
|
||
89 | * Begin the procedure to charge.
|
||
90 | **/
|
||
91 | void wl_recharge_begin(void) |
||
92 | { |
||
93 | 105 | bcoltin | recharge_state = POLLING; |
94 | 93 | jykong | station = -1;
|
95 | pollCount = 0;
|
||
96 | } |
||
97 | |||
98 | /**
|
||
99 | * Begin the procedure to stop charging. If we are currently docked,
|
||
100 | * we wait for departureDelay before alerting the station of our
|
||
101 | * departure. This is so that we have time to leave.
|
||
102 | **/
|
||
103 | void wl_recharge_stop()
|
||
104 | { |
||
105 | 105 | bcoltin | recharge_state = NOT_RECHARGING; |
106 | 93 | jykong | } |
107 | |||
108 | /**
|
||
109 | * Call when the robot has docked with the charging station.
|
||
110 | * This changes the state to DOCKED.
|
||
111 | **/
|
||
112 | void wl_recharge_dock(void) |
||
113 | { |
||
114 | 105 | bcoltin | if (recharge_state != SEEKING)
|
115 | 93 | jykong | { |
116 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Unexpected docking occurred.\n");
|
117 | 93 | jykong | return;
|
118 | } |
||
119 | |||
120 | 105 | bcoltin | recharge_state = DOCKED; |
121 | 93 | jykong | } |
122 | |||
123 | /**
|
||
124 | * Call this when the robot has departed from the charging
|
||
125 | * station. This changes the state to DEPARTING.
|
||
126 | **/
|
||
127 | void wl_recharge_depart(void) |
||
128 | { |
||
129 | 105 | bcoltin | if (recharge_state != DOCKED)
|
130 | 93 | jykong | { |
131 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Unexpected departure occurred.\n");
|
132 | 93 | jykong | return;
|
133 | } |
||
134 | |||
135 | 105 | bcoltin | recharge_state = DEPARTING; |
136 | 93 | jykong | } |
137 | |||
138 | /**
|
||
139 | * Returns the current state of the robot, as one of the constants
|
||
140 | * defined for a state -
|
||
141 | * NOT_CHARGING, POLLING, REQUESTING, SEEKING, DOCKED, or DEPARTING.
|
||
142 | *
|
||
143 | * @return the current state of the robot
|
||
144 | **/
|
||
145 | int wl_recharge_get_state(void) |
||
146 | { |
||
147 | 105 | bcoltin | return recharge_state;
|
148 | 93 | jykong | } |
149 | |||
150 | /**
|
||
151 | 100 | bcoltin | * Returns the station we are currently homing to, or -1
|
152 | * if no such station exists.
|
||
153 | *
|
||
154 | * @return the 16-bit XBee address of the charging station
|
||
155 | **/
|
||
156 | int wl_recharge_get_station(void) |
||
157 | { |
||
158 | return station;
|
||
159 | } |
||
160 | |||
161 | |||
162 | /**
|
||
163 | 93 | jykong | * Sends a request polling for charging stations.
|
164 | **/
|
||
165 | void wl_recharge_send_poll(void) |
||
166 | { |
||
167 | 105 | bcoltin | wl_send_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_POLL_STATIONS, |
168 | 116 | bcoltin | 0, 0, 0); |
169 | 93 | jykong | } |
170 | |||
171 | /**
|
||
172 | * Sends a request to seek a specific charging station.
|
||
173 | *
|
||
174 | * @param dest the charging station to seek
|
||
175 | **/
|
||
176 | void wl_recharge_send_request(int dest) |
||
177 | { |
||
178 | 105 | bcoltin | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_REQUEST, |
179 | 116 | bcoltin | 0, 0, dest, REQUEST_FRAME); |
180 | 93 | jykong | } |
181 | |||
182 | /**
|
||
183 | * Sends a request to verify what the charging station
|
||
184 | * believes we are doing.
|
||
185 | *
|
||
186 | * @param dest the charging station to check
|
||
187 | **/
|
||
188 | void wl_recharge_send_verify(int dest) |
||
189 | { |
||
190 | 105 | bcoltin | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_VERIFY, |
191 | 116 | bcoltin | 0, 0, dest, VERIFY_FRAME); |
192 | 93 | jykong | } |
193 | |||
194 | /**
|
||
195 | * Sends a packet to the charging station that we are
|
||
196 | * no longer attempting to charge from it.
|
||
197 | **/
|
||
198 | void wl_recharge_send_cancel(int dest) |
||
199 | { |
||
200 | 105 | bcoltin | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_CANCEL, |
201 | 116 | bcoltin | 0, 0, dest, CANCEL_FRAME); |
202 | 93 | jykong | } |
203 | |||
204 | /**
|
||
205 | * Sends a packet to the charging station that we are
|
||
206 | * attempting to dock with it.
|
||
207 | *
|
||
208 | * @param dest the charging station
|
||
209 | **/
|
||
210 | void wl_recharge_send_seeking(int dest) |
||
211 | { |
||
212 | 105 | bcoltin | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_SEEKING, |
213 | 116 | bcoltin | 0, 0, dest, SEEKING_FRAME); |
214 | 93 | jykong | } |
215 | |||
216 | /**
|
||
217 | * Sends a packet to the charging station that
|
||
218 | * we have docked with it.
|
||
219 | *
|
||
220 | * @param dest the charging station we have docked with
|
||
221 | **/
|
||
222 | void wl_recharge_send_docked(int dest) |
||
223 | { |
||
224 | 105 | bcoltin | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_DOCKED, |
225 | 116 | bcoltin | 0, 0, dest, DOCKED_FRAME); |
226 | 93 | jykong | } |
227 | |||
228 | /**
|
||
229 | * Called when this packet is unregistered with the wireless library.
|
||
230 | **/
|
||
231 | void wl_recharge_cleanup(void) |
||
232 | { |
||
233 | |||
234 | } |
||
235 | |||
236 | /**
|
||
237 | * Called when the timer goes off in the wireless library.
|
||
238 | **/
|
||
239 | void wl_recharge_timeout_handler(void) |
||
240 | { |
||
241 | 105 | bcoltin | switch (recharge_state)
|
242 | 93 | jykong | { |
243 | case POLLING:
|
||
244 | if (pollCount <= 0) |
||
245 | { |
||
246 | wl_recharge_send_poll(); |
||
247 | pollCount = POLL_DELAY + 1;
|
||
248 | } |
||
249 | pollCount--; |
||
250 | break;
|
||
251 | |||
252 | case REQUESTING:
|
||
253 | if (requestCount <= 0) |
||
254 | { |
||
255 | //request failed
|
||
256 | station = -1;
|
||
257 | 105 | bcoltin | recharge_state = POLLING; |
258 | 93 | jykong | break;
|
259 | } |
||
260 | requestCount--; |
||
261 | break;
|
||
262 | |||
263 | case SEEKING:
|
||
264 | if (verifyCount <= 0) |
||
265 | { |
||
266 | //station is down
|
||
267 | station = -1;
|
||
268 | 105 | bcoltin | recharge_state = POLLING; |
269 | 93 | jykong | break;
|
270 | } |
||
271 | if (verifyCount % VERIFY_FREQUENCY == 0 && verifyCount != VERIFY_DELAY) |
||
272 | wl_recharge_send_verify(station); |
||
273 | verifyCount--; |
||
274 | break;
|
||
275 | |||
276 | case DEPARTING:
|
||
277 | break;
|
||
278 | } |
||
279 | } |
||
280 | |||
281 | /**
|
||
282 | * Called when a response is received regarding a packet that was sent.
|
||
283 | *
|
||
284 | * @param frame the frame number for the packet
|
||
285 | * @param received true if the packet was received, false otherwise
|
||
286 | **/
|
||
287 | void wl_recharge_response_handler(int frame, int received) |
||
288 | { |
||
289 | if (received)
|
||
290 | return;
|
||
291 | if (!received)
|
||
292 | { |
||
293 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Sent packet not recieved.\n");
|
294 | 93 | jykong | switch (frame)
|
295 | { |
||
296 | case REQUEST_FRAME:
|
||
297 | break;
|
||
298 | case VERIFY_FRAME:
|
||
299 | break;
|
||
300 | case CANCEL_FRAME:
|
||
301 | case SEEKING_FRAME:
|
||
302 | case DOCKED_FRAME:
|
||
303 | break;
|
||
304 | } |
||
305 | } |
||
306 | } |
||
307 | |||
308 | /**
|
||
309 | * Handles receiving an error packet.
|
||
310 | *
|
||
311 | * @param type the packet type
|
||
312 | * @param source the 16-bit address of the packet's sender
|
||
313 | * @param packet the packet data
|
||
314 | * @param length the length in bytes of the packet
|
||
315 | **/
|
||
316 | void wl_recharge_handle_receive(char type, int source, unsigned char* packet, |
||
317 | int length)
|
||
318 | { |
||
319 | switch (type)
|
||
320 | { |
||
321 | case WL_RECHARGE_POLL_STATIONS:
|
||
322 | //since we aren't a station, we ignore this packet
|
||
323 | break;
|
||
324 | case WL_RECHARGE_STATION_AVAILABLE:
|
||
325 | wl_recharge_available_station(source); |
||
326 | break;
|
||
327 | 105 | bcoltin | case WL_RECHARGE_REQUEST:
|
328 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Only stations should receive WL_RECHARGE_REQUEST.\n");
|
329 | 93 | jykong | break;
|
330 | case WL_RECHARGE_REQUEST_ACCEPT:
|
||
331 | wl_recharge_request_accepted(source); |
||
332 | break;
|
||
333 | case WL_RECHARGE_VERIFY:
|
||
334 | wl_recharge_verify(source); |
||
335 | break;
|
||
336 | case WL_RECHARGE_CANCEL:
|
||
337 | wl_recharge_cancel(source); |
||
338 | break;
|
||
339 | case WL_RECHARGE_SEEKING:
|
||
340 | wl_recharge_seeking(source); |
||
341 | break;
|
||
342 | case WL_RECHARGE_DOCKED:
|
||
343 | wl_recharge_docked(source); |
||
344 | break;
|
||
345 | default:
|
||
346 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Error packet of unknown type received.\n");
|
347 | 93 | jykong | break;
|
348 | } |
||
349 | } |
||
350 | |||
351 | /**
|
||
352 | * Called when we receive a packet alerting us that
|
||
353 | * a station is available for charging.
|
||
354 | *
|
||
355 | * @param source the station which has available bays
|
||
356 | **/
|
||
357 | void wl_recharge_available_station(int source) |
||
358 | { |
||
359 | 105 | bcoltin | if (recharge_state != POLLING)
|
360 | 93 | jykong | return;
|
361 | station = source; |
||
362 | 105 | bcoltin | recharge_state = REQUESTING; |
363 | 93 | jykong | requestCount = REQUEST_DELAY; |
364 | wl_recharge_send_request(station); |
||
365 | } |
||
366 | |||
367 | /**
|
||
368 | * Called when we receive a packet saying our request
|
||
369 | * to charge has been accepted.
|
||
370 | *
|
||
371 | * @param source the station which has accepted our request
|
||
372 | **/
|
||
373 | void wl_recharge_request_accepted(int source) |
||
374 | { |
||
375 | 105 | bcoltin | if (recharge_state != REQUESTING)
|
376 | 93 | jykong | { |
377 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Accepted when we weren't requesting.\n");
|
378 | 93 | jykong | return;
|
379 | } |
||
380 | if (station != source)
|
||
381 | { |
||
382 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Accepted by a different station.\n");
|
383 | 93 | jykong | return;
|
384 | } |
||
385 | |||
386 | 105 | bcoltin | recharge_state = SEEKING; |
387 | 93 | jykong | verifyCount = VERIFY_DELAY; |
388 | } |
||
389 | |||
390 | /**
|
||
391 | * Called when we receive a request to verify our status
|
||
392 | * in the docking procedure.
|
||
393 | *
|
||
394 | * @param source the station verifying our request
|
||
395 | **/
|
||
396 | void wl_recharge_verify(int source) |
||
397 | { |
||
398 | if (source != station)
|
||
399 | { |
||
400 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Received verify from unassociated station.\n");
|
401 | 93 | jykong | wl_recharge_send_cancel(source); |
402 | return;
|
||
403 | } |
||
404 | |||
405 | 105 | bcoltin | switch (recharge_state)
|
406 | 93 | jykong | { |
407 | case SEEKING:
|
||
408 | wl_recharge_send_seeking(station); |
||
409 | break;
|
||
410 | case DOCKED:
|
||
411 | wl_recharge_send_docked(station); |
||
412 | break;
|
||
413 | default:
|
||
414 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Cancelled docking procedure.\n");
|
415 | 93 | jykong | wl_recharge_send_cancel(station); |
416 | break;
|
||
417 | } |
||
418 | } |
||
419 | |||
420 | /**
|
||
421 | * Called when we receive a packet cancelling our
|
||
422 | * docking procedure.
|
||
423 | *
|
||
424 | * @param source the station cancelling the procedure
|
||
425 | **/
|
||
426 | void wl_recharge_cancel(int source) |
||
427 | { |
||
428 | if (station != source)
|
||
429 | { |
||
430 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Received cancel from station we weren't docking with.\n");
|
431 | 93 | jykong | return;
|
432 | } |
||
433 | |||
434 | verifyCount = VERIFY_DELAY; |
||
435 | 105 | bcoltin | switch (recharge_state)
|
436 | 93 | jykong | { |
437 | case REQUESTING:
|
||
438 | station = -1;
|
||
439 | 105 | bcoltin | recharge_state = POLLING; |
440 | 93 | jykong | break;
|
441 | case SEEKING:
|
||
442 | station = -1;
|
||
443 | 105 | bcoltin | recharge_state = POLLING; |
444 | 93 | jykong | break;
|
445 | case DOCKED:
|
||
446 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Charging station kicked us out.\n");
|
447 | 93 | jykong | wl_recharge_depart(); |
448 | break;
|
||
449 | case DEPARTING:
|
||
450 | //ignore, since we are already departing
|
||
451 | break;
|
||
452 | default:
|
||
453 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Received unexpected cancellation.\n");
|
454 | 93 | jykong | break;
|
455 | } |
||
456 | } |
||
457 | |||
458 | /**
|
||
459 | * Called when we receive a packet stating that the
|
||
460 | * station believes we are seeking them.
|
||
461 | *
|
||
462 | * @param source the station we are seeking
|
||
463 | **/
|
||
464 | void wl_recharge_seeking(int source) |
||
465 | { |
||
466 | if (station != source)
|
||
467 | { |
||
468 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Received seeking alert from station we aren't seeking.\n");
|
469 | 93 | jykong | wl_recharge_send_cancel(source); |
470 | return;
|
||
471 | } |
||
472 | |||
473 | verifyCount = VERIFY_DELAY; |
||
474 | 105 | bcoltin | switch (recharge_state)
|
475 | 93 | jykong | { |
476 | case SEEKING:
|
||
477 | break;
|
||
478 | case DOCKED:
|
||
479 | case DEPARTING:
|
||
480 | wl_recharge_send_docked(station); |
||
481 | break;
|
||
482 | default:
|
||
483 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Received unexpected seeking confirmation.\n");
|
484 | 93 | jykong | break;
|
485 | } |
||
486 | } |
||
487 | |||
488 | /**
|
||
489 | * Called when we receive a packet stating that
|
||
490 | * we are docked with a station.
|
||
491 | *
|
||
492 | * @param source the station we are docked with
|
||
493 | **/
|
||
494 | void wl_recharge_docked(int source) |
||
495 | { |
||
496 | if (station != source)
|
||
497 | { |
||
498 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Unknown station believes we are docked.\n");
|
499 | 93 | jykong | wl_recharge_send_cancel(source); |
500 | return;
|
||
501 | } |
||
502 | |||
503 | verifyCount = VERIFY_DELAY; |
||
504 | 105 | bcoltin | switch (recharge_state)
|
505 | 93 | jykong | { |
506 | case DOCKED:
|
||
507 | case DEPARTING:
|
||
508 | break;
|
||
509 | case SEEKING:
|
||
510 | //not good - we can't immediately proceed to seeking again
|
||
511 | //we will leave the station and repoll it
|
||
512 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Seeking, but station believes we are docked. ");
|
513 | RECHARGE_DEBUG_PRINT("Leaving station and repolling.\n");
|
||
514 | 93 | jykong | wl_recharge_send_cancel(source); |
515 | wl_recharge_begin(); |
||
516 | break;
|
||
517 | default:
|
||
518 | 116 | bcoltin | RECHARGE_DEBUG_PRINT("Unexpected docking verification received.\n");
|
519 | 93 | jykong | break;
|
520 | } |
||
521 | } |