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