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