Revision 116
Updated recharging code for the robot, it actually runs, trying to fix problem with orbs in debugging mode.
| wl_recharge_group.c | ||
|---|---|---|
| 2 | 2 |
|
| 3 | 3 |
#include <wireless.h> |
| 4 | 4 |
#include <wl_defs.h> |
| 5 |
#include <stdio.h> |
|
| 6 |
#include <string.h> |
|
| 7 | 5 |
|
| 6 |
#include <serial.h> |
|
| 8 | 7 |
|
| 8 |
#include "recharge_defs.h" |
|
| 9 |
|
|
| 9 | 10 |
// Function prototypes |
| 10 | 11 |
void wl_recharge_timeout_handler(void); |
| 11 | 12 |
void wl_recharge_response_handler(int frame, int received); |
| ... | ... | |
| 59 | 60 |
int requestCount = 0; |
| 60 | 61 |
int verifyCount = 0; |
| 61 | 62 |
|
| 63 |
|
|
| 62 | 64 |
/** |
| 63 | 65 |
* Register this packet group with the wireless library. |
| 64 | 66 |
* This function must be called before any other wl_recharge |
| ... | ... | |
| 111 | 113 |
{
|
| 112 | 114 |
if (recharge_state != SEEKING) |
| 113 | 115 |
{
|
| 114 |
WL_DEBUG_PRINT("Unexpected docking occurred.\n");
|
|
| 116 |
RECHARGE_DEBUG_PRINT("Unexpected docking occurred.\n");
|
|
| 115 | 117 |
return; |
| 116 | 118 |
} |
| 117 | 119 |
|
| ... | ... | |
| 126 | 128 |
{
|
| 127 | 129 |
if (recharge_state != DOCKED) |
| 128 | 130 |
{
|
| 129 |
WL_DEBUG_PRINT("Unexpected departure occurred.\n");
|
|
| 131 |
RECHARGE_DEBUG_PRINT("Unexpected departure occurred.\n");
|
|
| 130 | 132 |
return; |
| 131 | 133 |
} |
| 132 | 134 |
|
| ... | ... | |
| 163 | 165 |
void wl_recharge_send_poll(void) |
| 164 | 166 |
{
|
| 165 | 167 |
wl_send_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_POLL_STATIONS, |
| 166 |
NULL, 0, 0);
|
|
| 168 |
0, 0, 0);
|
|
| 167 | 169 |
} |
| 168 | 170 |
|
| 169 | 171 |
/** |
| ... | ... | |
| 174 | 176 |
void wl_recharge_send_request(int dest) |
| 175 | 177 |
{
|
| 176 | 178 |
wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_REQUEST, |
| 177 |
NULL, 0, dest, REQUEST_FRAME);
|
|
| 179 |
0, 0, dest, REQUEST_FRAME);
|
|
| 178 | 180 |
} |
| 179 | 181 |
|
| 180 | 182 |
/** |
| ... | ... | |
| 186 | 188 |
void wl_recharge_send_verify(int dest) |
| 187 | 189 |
{
|
| 188 | 190 |
wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_VERIFY, |
| 189 |
NULL, 0, dest, VERIFY_FRAME);
|
|
| 191 |
0, 0, dest, VERIFY_FRAME);
|
|
| 190 | 192 |
} |
| 191 | 193 |
|
| 192 | 194 |
/** |
| ... | ... | |
| 196 | 198 |
void wl_recharge_send_cancel(int dest) |
| 197 | 199 |
{
|
| 198 | 200 |
wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_CANCEL, |
| 199 |
NULL, 0, dest, CANCEL_FRAME);
|
|
| 201 |
0, 0, dest, CANCEL_FRAME);
|
|
| 200 | 202 |
} |
| 201 | 203 |
|
| 202 | 204 |
/** |
| ... | ... | |
| 208 | 210 |
void wl_recharge_send_seeking(int dest) |
| 209 | 211 |
{
|
| 210 | 212 |
wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_SEEKING, |
| 211 |
NULL, 0, dest, SEEKING_FRAME);
|
|
| 213 |
0, 0, dest, SEEKING_FRAME);
|
|
| 212 | 214 |
} |
| 213 | 215 |
|
| 214 | 216 |
/** |
| ... | ... | |
| 220 | 222 |
void wl_recharge_send_docked(int dest) |
| 221 | 223 |
{
|
| 222 | 224 |
wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_DOCKED, |
| 223 |
NULL, 0, dest, DOCKED_FRAME);
|
|
| 225 |
0, 0, dest, DOCKED_FRAME);
|
|
| 224 | 226 |
} |
| 225 | 227 |
|
| 226 | 228 |
/** |
| ... | ... | |
| 288 | 290 |
return; |
| 289 | 291 |
if (!received) |
| 290 | 292 |
{
|
| 291 |
WL_DEBUG_PRINT("Sent packet not recieved.\n");
|
|
| 293 |
RECHARGE_DEBUG_PRINT("Sent packet not recieved.\n");
|
|
| 292 | 294 |
switch (frame) |
| 293 | 295 |
{
|
| 294 | 296 |
case REQUEST_FRAME: |
| ... | ... | |
| 323 | 325 |
wl_recharge_available_station(source); |
| 324 | 326 |
break; |
| 325 | 327 |
case WL_RECHARGE_REQUEST: |
| 326 |
WL_DEBUG_PRINT("Only stations should receive WL_RECHARGE_REQUEST.\n");
|
|
| 328 |
RECHARGE_DEBUG_PRINT("Only stations should receive WL_RECHARGE_REQUEST.\n");
|
|
| 327 | 329 |
break; |
| 328 | 330 |
case WL_RECHARGE_REQUEST_ACCEPT: |
| 329 | 331 |
wl_recharge_request_accepted(source); |
| ... | ... | |
| 341 | 343 |
wl_recharge_docked(source); |
| 342 | 344 |
break; |
| 343 | 345 |
default: |
| 344 |
WL_DEBUG_PRINT("Error packet of unknown type received.\n");
|
|
| 346 |
RECHARGE_DEBUG_PRINT("Error packet of unknown type received.\n");
|
|
| 345 | 347 |
break; |
| 346 | 348 |
} |
| 347 | 349 |
} |
| ... | ... | |
| 372 | 374 |
{
|
| 373 | 375 |
if (recharge_state != REQUESTING) |
| 374 | 376 |
{
|
| 375 |
WL_DEBUG_PRINT("Accepted when we weren't requesting.\n");
|
|
| 377 |
RECHARGE_DEBUG_PRINT("Accepted when we weren't requesting.\n");
|
|
| 376 | 378 |
return; |
| 377 | 379 |
} |
| 378 | 380 |
if (station != source) |
| 379 | 381 |
{
|
| 380 |
WL_DEBUG_PRINT("Accepted by a different station.\n");
|
|
| 382 |
RECHARGE_DEBUG_PRINT("Accepted by a different station.\n");
|
|
| 381 | 383 |
return; |
| 382 | 384 |
} |
| 383 | 385 |
|
| ... | ... | |
| 395 | 397 |
{
|
| 396 | 398 |
if (source != station) |
| 397 | 399 |
{
|
| 398 |
WL_DEBUG_PRINT("Received verify from unassociated station.\n");
|
|
| 400 |
RECHARGE_DEBUG_PRINT("Received verify from unassociated station.\n");
|
|
| 399 | 401 |
wl_recharge_send_cancel(source); |
| 400 | 402 |
return; |
| 401 | 403 |
} |
| ... | ... | |
| 409 | 411 |
wl_recharge_send_docked(station); |
| 410 | 412 |
break; |
| 411 | 413 |
default: |
| 412 |
WL_DEBUG_PRINT("Cancelled docking procedure.\n");
|
|
| 414 |
RECHARGE_DEBUG_PRINT("Cancelled docking procedure.\n");
|
|
| 413 | 415 |
wl_recharge_send_cancel(station); |
| 414 | 416 |
break; |
| 415 | 417 |
} |
| ... | ... | |
| 425 | 427 |
{
|
| 426 | 428 |
if (station != source) |
| 427 | 429 |
{
|
| 428 |
WL_DEBUG_PRINT("Received cancel from station we weren't docking with.\n");
|
|
| 430 |
RECHARGE_DEBUG_PRINT("Received cancel from station we weren't docking with.\n");
|
|
| 429 | 431 |
return; |
| 430 | 432 |
} |
| 431 | 433 |
|
| ... | ... | |
| 441 | 443 |
recharge_state = POLLING; |
| 442 | 444 |
break; |
| 443 | 445 |
case DOCKED: |
| 444 |
WL_DEBUG_PRINT("Charging station kicked us out.\n");
|
|
| 446 |
RECHARGE_DEBUG_PRINT("Charging station kicked us out.\n");
|
|
| 445 | 447 |
wl_recharge_depart(); |
| 446 | 448 |
break; |
| 447 | 449 |
case DEPARTING: |
| 448 | 450 |
//ignore, since we are already departing |
| 449 | 451 |
break; |
| 450 | 452 |
default: |
| 451 |
WL_DEBUG_PRINT("Received unexpected cancellation.\n");
|
|
| 453 |
RECHARGE_DEBUG_PRINT("Received unexpected cancellation.\n");
|
|
| 452 | 454 |
break; |
| 453 | 455 |
} |
| 454 | 456 |
} |
| ... | ... | |
| 463 | 465 |
{
|
| 464 | 466 |
if (station != source) |
| 465 | 467 |
{
|
| 466 |
WL_DEBUG_PRINT("Received seeking alert from station we aren't seeking.\n");
|
|
| 468 |
RECHARGE_DEBUG_PRINT("Received seeking alert from station we aren't seeking.\n");
|
|
| 467 | 469 |
wl_recharge_send_cancel(source); |
| 468 | 470 |
return; |
| 469 | 471 |
} |
| ... | ... | |
| 478 | 480 |
wl_recharge_send_docked(station); |
| 479 | 481 |
break; |
| 480 | 482 |
default: |
| 481 |
WL_DEBUG_PRINT("Received unexpected seeking confirmation.\n");
|
|
| 483 |
RECHARGE_DEBUG_PRINT("Received unexpected seeking confirmation.\n");
|
|
| 482 | 484 |
break; |
| 483 | 485 |
} |
| 484 | 486 |
} |
| ... | ... | |
| 493 | 495 |
{
|
| 494 | 496 |
if (station != source) |
| 495 | 497 |
{
|
| 496 |
WL_DEBUG_PRINT("Unknown station believes we are docked.\n");
|
|
| 498 |
RECHARGE_DEBUG_PRINT("Unknown station believes we are docked.\n");
|
|
| 497 | 499 |
wl_recharge_send_cancel(source); |
| 498 | 500 |
return; |
| 499 | 501 |
} |
| ... | ... | |
| 507 | 509 |
case SEEKING: |
| 508 | 510 |
//not good - we can't immediately proceed to seeking again |
| 509 | 511 |
//we will leave the station and repoll it |
| 510 |
WL_DEBUG_PRINT("Seeking, but station believes we are docked. ");
|
|
| 511 |
WL_DEBUG_PRINT("Leaving station and repolling.\n");
|
|
| 512 |
RECHARGE_DEBUG_PRINT("Seeking, but station believes we are docked. ");
|
|
| 513 |
RECHARGE_DEBUG_PRINT("Leaving station and repolling.\n");
|
|
| 512 | 514 |
wl_recharge_send_cancel(source); |
| 513 | 515 |
wl_recharge_begin(); |
| 514 | 516 |
break; |
| 515 | 517 |
default: |
| 516 |
WL_DEBUG_PRINT("Unexpected docking verification received.\n");
|
|
| 518 |
RECHARGE_DEBUG_PRINT("Unexpected docking verification received.\n");
|
|
| 517 | 519 |
break; |
| 518 | 520 |
} |
| 519 | 521 |
} |
Also available in: Unified diff