src/robofsm/competition.cc

   1 /*
   2  * fsmmain.cc       09/04/..
   3  *
   4  * Robot's main control program (Eurobot 2009).
   5  *
   6  * Copyright: (c) 2009 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 #ifndef DEBUG
  12 #define DEBUG
  13 #endif
  14
  15 #define FSM_MAIN
  16 #include <robodata.h>
  17 #include <robot.h>
  18 #include <fsm.h>
  19 #include <unistd.h>
  20 #include <math.h>
  21 #include <movehelper.h>
  22 #include <map.h>
  23 #include <sharp.h>
  24 #include <robomath.h>
  25 #include <string.h>
  26 #include <robodim.h>
  27 #include <stdbool.h>
  28 #include <error.h>
  29 #include <actuators.h>
  30
  31 #ifdef COMPETITION
  32 #define WAIT_FOR_START
  33 #define COMPETITION_TIME_DEFAULT        90
  34 #define TIME_TO_DEPOSITE_DEFAULT        60
  35 #else
  36 #undef WAIT_FOR_START
  37 #define COMPETITION_TIME_DEFAULT        900
  38 #define TIME_TO_DEPOSITE_DEFAULT        60
  39 #endif
  40
  41 /* competition time in seconds */
  42 #define COMPETITION_TIME        COMPETITION_TIME_DEFAULT
  43 #define TIME_TO_DEPOSITE        TIME_TO_DEPOSITE_DEFAULT
  44 /* competition time in seconds */
  45
  46 /************************************************************************
  47  * SUBFSM's return values ...
  48  ************************************************************************/
  49
  50 typedef enum {
  51         LOAD_SUCCESS = 0,
  52         LOAD_FAIL,
  53 } subfsm_ret_val;
  54
  55 #define FSM_EVENT_RET_VAL ((subfsm_ret_val)FSM_EVENT_INT)
  56
  57 /************************************************************************
  58  * Trajectory constraints used, are initialized in the init state
  59  ************************************************************************/
  60
  61 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
  62
  63 /************************************************************************
  64  * Variables (short_time_to_end and  variables related to puck collecting)
  65  ************************************************************************/
  66
  67 int free_puck_to_try_to_get_next; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
  68 const int pucks_at_once = 4; // number of pucks to load at once (maximum number of pucks we want to have in robot)
  69 bool short_time_to_end;
  70
  71 /************************************************************************
  72  * Definition of particular "free pucks pick up sequences" and related stuff
  73  ************************************************************************/
  74
  75 // FIXME: has to be redefined and tested
  76 const int free_puck_pick_up_sequence[][6][3] = {
  77 /* nx, ny, arrival angle */
  78         { /* lot 1 */
  79                 {0, 3, 135},
  80                 {1, 3, 180},
  81                 {2, 3, 180},
  82                 {2, 0, 90},
  83                 {1, 0, 45},
  84                 {0, 0, 0},
  85         },
  86         {
  87                 {0, 3, 135},
  88                 {2, 3, 90},
  89                 {2, 2, 90},
  90                 {2, 1, 90},
  91                 {2, 0, 90},
  92                 {0, 0, 10},
  93         },
  94         {
  95                 {0, 3, 125},
  96                 {2, 3, 160},
  97                 {1, 2, 40},
  98                 {1, 1, 90},
  99                 {2, 0, 135},
 100                 {0, 0, 0},
 101         },
 102         { /* lot 4 */
 103                 {0, 3, 135},
 104                 {0, 2, 90},
 105                 {0, 1, 90},
 106                 {2, 0, 130},
 107                 {2, 3, -90},
 108                 {0, 0, 90},
 109         },
 110         {
 111                 {0, 3, 135},
 112                 {1, 3, 180},
 113                 {2, 2, 135},
 114                 {2, 1, 90},
 115                 {1, 0, 0},
 116                 {0, 0, 0},
 117         },
 118         { /* lot 6 */
 119                 {0, 3, 135},
 120                 {1, 3, 180},
 121                 {1, 2, 90},
 122                 {1, 1, 90},
 123                 {1, 0, 0},
 124                 {0, 0, 0},
 125         },
 126         {
 127                 {0, 3, 135},
 128                 {0, 2, 90},
 129                 {0, 1, 90},
 130                 {1, 0, 135},
 131                 {1, 3, -45},
 132                 {0, 0, 45},
 133         },
 134         { /* lot 8 */
 135                 {0, 3, 135},
 136                 {1, 2, 135},
 137                 {1, 1, 90},
 138                 {0, 0, 45},
 139                 {2, 2, -90},
 140                 {2, 1, -90},
 141         },
 142         {
 143                 {0, 3, 135},
 144                 {0, 2, 90},
 145                 {0, 1, 90},
 146                 {0, 0, 90},
 147                 {2, 2, -90},
 148                 {2, 1, -90},
 149         },
 150         { /* lot 10 */
 151                 {0, 3, 135},
 152                 {0, 2, 90},
 153                 {0, 1, 90},
 154                 {0, 0, 90},
 155                 {1, 1, -45},
 156                 {1, 2, -90},
 157         },
 158 };
 159
 160 const int preferred_acropolis_approach_angles[][3] = {
 161         {180, 180, 220},
 162         {130, 160, 200},
 163         {130, 160, 200},
 164         {130, 160, 200}, // FIXME... fix these
 165         {130, 160, 200}, // FIXME... fix these
 166         {130, 160, 200}, // FIXME... fix these
 167         {130, 160, 200}, // FIXME... fix these
 168         {130, 160, 200}, // FIXME... fix these
 169         {130, 160, 200}, // FIXME... fix these
 170         {130, 160, 200}, // FIXME... fix these
 171 };
 172
 173 /*
 174 const int universal_free_puck_pick_up_sequence[12][3] = {
 175                 {0, 3, 135},
 176                 {0, 2, 90},
 177                 {0, 1, 90},
 178                 {0, 0, 90},
 179                 {2, 0, 260},
 180                 {2, 1, 270},
 181                 {2, 2, 270},
 182                 {2, 3, 270},
 183                 {1, 3, 80},
 184                 {1, 2, 90},
 185                 {1, 1, 90},
 186                 {1, 0, 90},
 187 };
 188
 189 const int preferred_universal_acropolis_approach_angles[][3] = {
 190 }; */
 191
 192 /************************************************************************
 193  * NOTES ON DATA WE NEED TO STORE
 194  ************************************************************************/
 195
 196 /*
 197 PLAYGROUND:
 198  - puck (column element) dispensers status (number of pucks)
 199         - their max. capacity is 5
 200  - lintel dispensers status
 201  - free column elements configuration and their positions
 202  - free pucks configuration (lot number)
 203 ROBOT:
 204  - puck stack status (puck count)
 205  - lintel holder status
 206  */
 207
 208 /************************************************************************
 209  * MISC FUNCTIONS
 210  ************************************************************************/
 211
 212 /**
 213  * Competition timer. Stop robot when the timer exceeds.
 214  *
 215  */
 216 void *timing_thread(void *arg)
 217 {
 218         struct timespec start;
 219
 220         clock_gettime(CLOCK_MONOTONIC, &start);
 221 #define WAIT(sec)                                                       \
 222         do {                                                            \
 223                 struct timespec t;                                      \
 224                 t.tv_sec = start.tv_sec+sec;                            \
 225                 t.tv_nsec = start.tv_nsec;                              \
 226                 clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); \
 227         } while(0)
 228
 229         WAIT(5);
 230         robot.use_back_switch = true;
 231         printf("Back switch not ignored\n");
 232
 233         WAIT(TIME_TO_DEPOSITE);
 234         short_time_to_end = true;
 235
 236         WAIT(COMPETITION_TIME);
 237         printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
 238         robot_exit();
 239
 240         return NULL;
 241 }
 242
 243 void robot_goto_acropolis(int phi)
 244 {
 245         robot_moveto_trans(
 246                 ACROPOLIS_CENTER_X + (ACROPOLIS_RADIUS + ROBOT_AXIS_TO_PUCK_M + 0.07)*cos(DEG2RAD(phi)),
 247                 ACROPOLIS_CENTER_Y + (ACROPOLIS_RADIUS + ROBOT_AXIS_TO_PUCK_M + 0.07)*sin(DEG2RAD(phi)),
 248                 ARRIVE_FROM(DEG2RAD(phi+180), 0.07), &tcSlow);
 249 }
 250
 251 void robot_goto_puck_in_grid(int nx, int ny, int phi)
 252 {
 253         struct puck_pos pp = free_puck_pos(nx, ny); // puck position
 254         robot_moveto_trans(
 255                 pp.x + ROBOT_AXIS_TO_PUCK_M*cos(DEG2RAD(phi)),
 256                 pp.y + ROBOT_AXIS_TO_PUCK_M*sin(DEG2RAD(phi)),
 257                 ARRIVE_FROM(DEG2RAD(phi+180), 0.1), &tcSlow);
 258 }
 259
 260 void robot_goto_next_puck_in_sequence(int lot, int puck_number)
 261 {
 262         robot_goto_puck_in_grid(
 263                 free_puck_pick_up_sequence[lot][puck_number][0],
 264                 free_puck_pick_up_sequence[lot][puck_number][1],
 265                 free_puck_pick_up_sequence[lot][puck_number][2]);
 266 }
 267
 268 /************************************************************************
 269  * FSM STATES DECLARATION
 270  ************************************************************************/
 271
 272 /* initial and starting states */
 273 FSM_STATE_DECL(init);
 274 FSM_STATE_DECL(wait_for_start);
 275 /* strategies related states */
 276 FSM_STATE_DECL(decide_what_next);
 277 FSM_STATE_DECL(collect_free_pucks);
 278 FSM_STATE_DECL(deposit_at_acropolis);
 279 /* movement states */
 280 FSM_STATE_DECL(approach_acropolis);
 281 FSM_STATE_DECL(simple_construction_zone_approach);
 282 FSM_STATE_DECL(approach_our_static_dispenser);
 283 FSM_STATE_DECL(approach_opponents_static_dispenser);
 284 /* States handling ACT's actions (SUBFSMs) */
 285 FSM_STATE_DECL(load_the_puck);
 286 FSM_STATE_DECL(look_around_for_puck);
 287
 288 /************************************************************************
 289  * INITIAL AND STARTING STATES
 290  ************************************************************************/
 291
 292 FSM_STATE(init)
 293 {
 294         switch (FSM_EVENT) {
 295         case EV_ENTRY:
 296                 short_time_to_end = false;
 297                 act_camera_on();
 298                 free_puck_to_try_to_get_next = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
 299                 tcFast = trajectoryConstraintsDefault;
 300                 //tcFast.maxv = 1.5;
 301                 tcSlow = trajectoryConstraintsDefault;
 302                 tcSlow.maxv = 0.2;
 303                 tcSlow.maxacc = 0.1;
 304                 tcSlow.maxomega = 1;
 305                 tcSlow.maxangacc = 1;
 306                 tcVerySlow = trajectoryConstraintsDefault;
 307                 tcVerySlow.maxv =  0.1;
 308                 tcVerySlow.maxacc = 0.05;
 309                 tcVerySlow.maxomega = 0.7;
 310                 tcVerySlow.maxangacc = 1;
 311                 FSM_TRANSITION(wait_for_start);
 312                 break;
 313         default:
 314                 break;
 315         }
 316 }
 317
 318 FSM_STATE(wait_for_start)
 319 {
 320         pthread_t thid;
 321 #ifdef COMPETITION
 322         printf("COMPETITION mode set\n");
 323 #endif
 324         switch (FSM_EVENT) {
 325 #ifdef WAIT_FOR_START
 326                 case EV_ENTRY:
 327                         break;
 328                 case EV_START:
 329 #else
 330                 case EV_ENTRY:
 331                 case EV_START:
 332 #endif
 333                         act_camera_off();
 334                         /* start competition timer */
 335                         pthread_create(&thid, NULL, timing_thread, NULL);
 336                         robot_set_est_pos_trans(0.16,
 337                                                 PLAYGROUND_HEIGHT_M - 0.16,
 338                                                 DEG2RAD(-45));
 339                         FSM_TRANSITION(decide_what_next);
 340                         break;
 341                 case EV_TIMER:
 342                 case EV_RETURN:
 343                 case EV_LASER_POWER:
 344                 case EV_GOAL_NOT_REACHABLE: // currently not used
 345                 //case EV_SHORT_TIME_TO_END:
 346                 case EV_STACK_FULL:
 347                 case EV_ACTION_DONE:
 348                 case EV_ACTION_ERROR:
 349                 case EV_PUCK_REACHABLE:
 350                 case EV_MOTION_DONE:
 351                         DBG_PRINT_EVENT("unhandled event");
 352                         break;
 353                 case EV_EXIT:
 354                         break;
 355         }
 356 }
 357
 358 /************************************************************************
 359  * STRATEGIES RELATED STATES
 360  ************************************************************************/
 361
 362 FSM_STATE(decide_what_next)
 363 {
 364         switch (FSM_EVENT) {
 365                 case EV_ENTRY:
 366                         if(short_time_to_end) {
 367                                 if(robot.pucks_inside > 0) {
 368                                         FSM_TRANSITION(deposit_at_acropolis);
 369                                 }
 370                         }
 371                         if(robot.pucks_inside == pucks_at_once) {
 372                                 FSM_TRANSITION(deposit_at_acropolis);
 373                         } else {
 374                                 if(robot.pucks_inside < pucks_at_once && free_puck_to_try_to_get_next < 6) {
 375                                         FSM_TRANSITION(collect_free_pucks);
 376                                 }
 377                         }
 378                         break;
 379                 case EV_MOTION_DONE:
 380                 case EV_ACTION_DONE:
 381                 case EV_RETURN:
 382                 case EV_TIMER:
 383                 case EV_LASER_POWER:
 384                 case EV_GOAL_NOT_REACHABLE: // currently not used
 385                 //case EV_SHORT_TIME_TO_END:
 386                 case EV_STACK_FULL:
 387                 case EV_ACTION_ERROR:
 388                 case EV_PUCK_REACHABLE:
 389                 case EV_START:
 390                         DBG_PRINT_EVENT("unhandled event");
 391                         break;
 392                 case EV_EXIT:
 393                         break;
 394         }
 395 }
 396
 397 FSM_STATE(deposit_at_acropolis)
 398 {
 399         static int deposit_status;
 400         switch (FSM_EVENT) {
 401                 case EV_ENTRY:
 402                         deposit_status = 0;
 403                         SUBFSM_TRANSITION(approach_acropolis, NULL);
 404                         break;
 405                 case EV_RETURN: {
 406                         //if ((bool)FSM_EVENT_INT == true) {
 407                                 int floor = 2;
 408                                 FSM_SIGNAL(ACT, EV_PREPARE_THE_UNLOAD, (void*)floor);
 409                         //}
 410                         }
 411                         break;
 412                 case EV_ACTION_DONE:
 413                         switch(deposit_status) {
 414                         case 0:
 415                                 robot_move_by(0.11, NO_TURN(), &tcSlow);
 416                                 break;
 417                         case 1:
 418                                 robot_move_by(-0.07, NO_TURN(), &tcSlow);
 419                                 break;
 420                         }
 421                         deposit_status++;
 422                         break;
 423                 case EV_MOTION_DONE:
 424                         switch(deposit_status) {
 425                         case 1:
 426                                 FSM_SIGNAL(ACT, EV_UNLOAD_PUCKS, NULL);
 427                                 break;
 428                         case 2:
 429                                 robot_move_by(0.08, NO_TURN(), &tcVerySlow);
 430                                 deposit_status++;
 431                                 break;
 432                         case 3:
 433                                 robot_move_by(-0.15, NO_TURN(), &tcSlow);
 434                                 deposit_status++;
 435                                 break;
 436                         case 4:
 437                                 FSM_SIGNAL(ACT, EV_FREE_SPACE, NULL);
 438                                 FSM_TRANSITION(decide_what_next);
 439                                 deposit_status++;
 440                         }
 441                         break;
 442                 // case EV_SHORT_TIME_TO_END:
 443                 case EV_TIMER:
 444                 case EV_LASER_POWER:
 445                 case EV_GOAL_NOT_REACHABLE: // currently not used
 446                 case EV_STACK_FULL:
 447                 case EV_ACTION_ERROR:
 448                 case EV_PUCK_REACHABLE:
 449                 case EV_START:
 450                         DBG_PRINT_EVENT("unhandled event");
 451                         break;
 452                 case EV_EXIT:
 453                         break;
 454         }
 455 }
 456
 457 FSM_STATE(collect_free_pucks)
 458 {
 459         // game configuration... detected lot number is stored in robot.game_conf
 460         switch (FSM_EVENT) {
 461                 case EV_ENTRY:
 462                         robot_goto_next_puck_in_sequence(robot.game_conf, free_puck_to_try_to_get_next);
 463                         break;
 464                 case EV_MOTION_DONE: {
 465                                 printf("-----arrived where the free puck no. %d should be\n", free_puck_to_try_to_get_next);
 466                                 SUBFSM_TRANSITION(load_the_puck, NULL);
 467                         }
 468                         break;
 469                 case EV_RETURN:
 470                         switch(FSM_EVENT_RET_VAL) {
 471                         case LOAD_SUCCESS:
 472                                 printf(">>>>>> Loading the puck succeeded\n");
 473                                 break;
 474                         case LOAD_FAIL:
 475                                 printf(">>>>>> Loading the puck FAILED\n");
 476                                 break;
 477                         }
 478                         free_puck_to_try_to_get_next++;
 479                         FSM_TRANSITION(decide_what_next);
 480                         break;
 481                 //case EV_SHORT_TIME_TO_END:
 482                 case EV_PUCK_REACHABLE:
 483                 case EV_ACTION_DONE:
 484                 case EV_TIMER:
 485                 case EV_LASER_POWER:
 486                 case EV_GOAL_NOT_REACHABLE: // currently not used
 487                 case EV_STACK_FULL:
 488                 case EV_ACTION_ERROR:
 489                 case EV_START:
 490                         DBG_PRINT_EVENT("unhandled event");
 491                         break;
 492                 case EV_EXIT:
 493                         break;
 494         }
 495 }
 496
 497 /************************************************************************
 498  * MOVEMENT STATES
 499  ************************************************************************/
 500
 501 FSM_STATE(approach_acropolis)
 502 {
 503         switch (FSM_EVENT) {
 504                 case EV_ENTRY:
 505                         robot_goto_acropolis(preferred_acropolis_approach_angles[(int)robot.game_conf][(free_puck_to_try_to_get_next-1)/2]);
 506                         break;
 507                 case EV_MOTION_DONE:
 508                         SUBFSM_RET(NULL);
 509                         // FIXME SUBFSM_RET((void*)true);
 510                         break;
 511                 case EV_GOAL_NOT_REACHABLE: // currently not used
 512                         break;
 513                 //case EV_SHORT_TIME_TO_END:
 514                 case EV_ACTION_DONE:
 515                 case EV_RETURN:
 516                 case EV_TIMER:
 517                 case EV_LASER_POWER:
 518                 case EV_STACK_FULL:
 519                 case EV_ACTION_ERROR:
 520                 case EV_PUCK_REACHABLE:
 521                 case EV_START:
 522                         DBG_PRINT_EVENT("unhandled event");
 523                         break;
 524                 case EV_EXIT:
 525                         break;
 526         }
 527 }
 528
 529 FSM_STATE(simple_construction_zone_approach)
 530 {
 531         switch (FSM_EVENT) {
 532                 case EV_ENTRY:
 533                         robot_trajectory_new(&tcFast);
 534                         robot_trajectory_add_point_trans(0.9, 1);
 535                         robot_trajectory_add_final_point_trans(0.9, ROBOT_AXIS_TO_FRONT_M + 0.05, NO_TURN());
 536                         break;
 537                 case EV_MOTION_DONE:
 538                         //FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
 539                         break;
 540                 //case EV_SHORT_TIME_TO_END:
 541                 case EV_ACTION_DONE:
 542                 case EV_RETURN:
 543                 case EV_TIMER:
 544                 case EV_LASER_POWER:
 545                 case EV_GOAL_NOT_REACHABLE: // currently not used
 546                 case EV_STACK_FULL:
 547                 case EV_ACTION_ERROR:
 548                 case EV_PUCK_REACHABLE:
 549                 case EV_START:
 550                         DBG_PRINT_EVENT("unhandled event");
 551                         break;
 552                 case EV_EXIT:
 553                         break;
 554         }
 555 }
 556
 557 FSM_STATE(approach_our_static_dispenser)
 558 {
 559         switch (FSM_EVENT) {
 560                 case EV_ENTRY:  {
 561                         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 562                         tc.maxv /= 1.0;
 563                         robot_trajectory_new(&tc);
 564
 565                         robot_trajectory_add_point_trans(STATIC_DISPENSER_X,
 566                                                          STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
 567                         robot_trajectory_add_final_point_trans(STATIC_DISPENSER_X,
 568                                                                STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
 569                                                                NO_TURN());
 570                         }
 571                         break;
 572                 case EV_MOTION_DONE:
 573                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 574                         printf("Arrived to the static dispenser\n");
 575                         break;
 576                 case EV_ACTION_DONE:
 577                         printf("A Puck picked up\n");
 578                         break;
 579                 //case EV_SHORT_TIME_TO_END:
 580                 case EV_RETURN:
 581                 case EV_TIMER:
 582                 case EV_LASER_POWER:
 583                 case EV_GOAL_NOT_REACHABLE: // currently not used
 584                 case EV_STACK_FULL:
 585                 case EV_ACTION_ERROR:
 586                 case EV_PUCK_REACHABLE:
 587                 case EV_START:
 588                         DBG_PRINT_EVENT("unhandled event");
 589                         break;
 590                 case EV_EXIT:
 591                         break;
 592         }
 593 }
 594
 595 FSM_STATE(approach_opponents_static_dispenser)
 596 {
 597         switch (FSM_EVENT) {
 598                 case EV_ENTRY:  {
 599                         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 600                         tc.maxv /= 1.0;
 601                         robot_trajectory_new(&tc);
 602
 603                         robot_trajectory_add_point_trans(OPPONENTS_STATIC_DISPENSER_X,
 604                                                          OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
 605                         robot_trajectory_add_final_point_trans(OPPONENTS_STATIC_DISPENSER_X,
 606                                                                OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
 607                                                                NO_TURN());
 608                         }
 609                         break;
 610                 case EV_MOTION_DONE:
 611                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 612                         printf("Arrived to the static dispenser\n");
 613                         break;
 614                 case EV_ACTION_DONE:
 615                         printf("A Puck picked up\n");
 616                         break;
 617                 //case EV_SHORT_TIME_TO_END:
 618                 case EV_RETURN:
 619                 case EV_TIMER:
 620                 case EV_LASER_POWER:
 621                 case EV_GOAL_NOT_REACHABLE: // currently not used
 622                 case EV_STACK_FULL:
 623                 case EV_ACTION_ERROR:
 624                 case EV_PUCK_REACHABLE:
 625                 case EV_START:
 626                         DBG_PRINT_EVENT("unhandled event");
 627                         break;
 628                 case EV_EXIT:
 629                         break;
 630         }
 631 }
 632
 633 /************************************************************************
 634  * STATES HANDLING ACT's ACTIONS (to be used as SUB FSMs)
 635  ************************************************************************/
 636
 637 FSM_STATE(load_the_puck)
 638 {
 639         static int puck_load_attempt_count;
 640         switch (FSM_EVENT) {
 641                 case EV_ENTRY:
 642                         puck_load_attempt_count = 0;
 643                         FSM_SIGNAL(ACT, EV_SCRABBLE, NULL);
 644                         FSM_TIMER(200);
 645                         break;
 646                 case EV_TIMER:
 647                         robot_move_by(0.02, NO_TURN(), &tcSlow);
 648                         break;
 649                 case EV_MOTION_DONE:
 650                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 651                         break;
 652                 case EV_ACTION_DONE:
 653                         SUBFSM_RET((void *)LOAD_SUCCESS);
 654                         break;
 655                 case EV_ACTION_ERROR:
 656                         puck_load_attempt_count++;
 657                         if (puck_load_attempt_count > 1) {
 658                                 SUBFSM_RET((void *)LOAD_FAIL);
 659                         } else {
 660                                 robot_move_by(0.02, NO_TURN(), &tcSlow);
 661                         }
 662                         break;
 663                 //case EV_SHORT_TIME_TO_END:
 664                 case EV_RETURN:
 665                 case EV_LASER_POWER:
 666                 case EV_GOAL_NOT_REACHABLE: // currently not used
 667                 case EV_STACK_FULL:
 668                 case EV_PUCK_REACHABLE:
 669                 case EV_START:
 670                         DBG_PRINT_EVENT("unhandled event");
 671                         break;
 672                 case EV_EXIT:
 673                         break;
 674         }
 675 }
 676
 677 int main()
 678 {
 679         int rv;
 680
 681         rv = robot_init();
 682         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 683
 684         robot.fsm.main.debug_states = 1;
 685         robot.fsm.motion.debug_states = 1;
 686         robot.fsm.act.debug_states = 1;
 687
 688         robot.fsm.main.state = &fsm_state_main_init;
 689
 690         rv = robot_start();
 691         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 692
 693         robot_destroy();
 694
 695         return 0;
 696 }