src/robofsm/fsmmain.c

   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
  29 #ifdef COMPETITION
  30 #define WAIT_FOR_START
  31 #define COMPETITION_TIME_DEFAULT        90
  32 #define TIME_TO_DEPOSITE_DEFAULT        60
  33 #else
  34 #undef WAIT_FOR_START
  35 #define COMPETITION_TIME_DEFAULT        900
  36 #define TIME_TO_DEPOSITE_DEFAULT        60
  37 #endif
  38
  39 /* competition time in seconds */
  40 #define COMPETITION_TIME        COMPETITION_TIME_DEFAULT
  41 #define TIME_TO_DEPOSITE        TIME_TO_DEPOSITE_DEFAULT
  42 /* competition time in seconds */
  43
  44 /************************************************************************
  45  * SUBFSM's return values ...
  46  ************************************************************************/
  47
  48 typedef enum {
  49         LOAD_SUCCESS = 0,
  50         LOAD_FAIL,
  51         //LOOK_AROUND_SUCCESS,  // obsolete, no sharp sensor
  52         //LOOK_AROUND_FAIL      // obsolete, no sharp sensor
  53 } SUBFSM_RET_VAL;
  54
  55 #define FSM_EVENT_RET_VAL ((SUBFSM_RET_VAL)FSM_EVENT_PTR)
  56
  57 /************************************************************************
  58  * Trajectory constraints used, are initialized in the init state
  59  ************************************************************************/
  60
  61 struct TrajectoryConstraints tcFast, tcSlow;
  62
  63 /************************************************************************
  64  * Variables related to puck collecting
  65  ************************************************************************/
  66
  67 int free_puck_to_try_to_get_next = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
  68
  69 /************************************************************************
  70  * Definition of particular "free pucks pick up sequences"
  71  ************************************************************************/
  72
  73 const int free_puck_pick_up_sequence[][6][3] = {
  74         { /* lot 1 */
  75                 {0, 3, 135},
  76                 {1, 3, 180},
  77                 {2, 3, 180},
  78                 {2, 0, 90},
  79                 {1, 0, 45},
  80                 {0, 0, 0}, // FIXME: test last two pucks
  81         },
  82         {
  83                 {0, 3, 135},
  84                 {2, 3, 90},
  85                 {2, 2, 90},
  86                 {2, 1, 90},
  87                 {2, 0, 90},
  88                 {0, 0, 10}, // FIXME: test last two pucks
  89         },
  90         {
  91                 {0, 3, 135},
  92                 {2, 3, 160},
  93                 {1, 2, 40},
  94                 {1, 1, 90},
  95                 {2, 0, 135},
  96                 {0, 0, 0},   // FIXME: test last two pucks
  97         },
  98         { /* lot 4 */
  99                 {0, 3, 135},
 100                 {0, 2, 90},
 101                 {0, 1, 90},
 102                 {0, 0, 90},
 103                 {2, 3, -90},
 104                 {2, 0, 90}, // FIXME: test last two pucks
 105         },
 106         {
 107                 {0, 3, 135},
 108                 {1, 3, 180},
 109                 {2, 2, 135},
 110                 {2, 1, 90},
 111                 {1, 0, 0},
 112                 {0, 0, 0}, // FIXME: test last two pucks
 113         },
 114         { /* lot 6 */
 115                 {0, 3, 135},
 116                 {1, 3, 180},
 117                 {1, 2, 135},
 118                 {1, 1, 90},
 119                 {1, 0, 0},
 120                 {0, 0, 0}, // FIXME: test last two pucks
 121         },
 122         {
 123                 {0, 3, 135},
 124                 {0, 2, 90},
 125                 {0, 1, 90},
 126                 {0, 0, 90},
 127                 {1, 3, -45},
 128                 {1, 0, 45}, // FIXME: test last two pucks
 129         },
 130         { /* lot 8 */
 131                 {0, 3, 135},
 132                 {1, 2, 135},
 133                 {1, 1, 90},
 134                 {0, 0, 45},
 135                 {2, 2, -90},
 136                 {2, 1, -90}, // FIXME: test last two pucks
 137         },
 138         {
 139                 {0, 3, 135},
 140                 {0, 2, 90},
 141                 {0, 1, 90},
 142                 {0, 0, 90},
 143                 {2, 2, -90},
 144                 {2, 1, -90}, // FIXME: test last two pucks
 145         },
 146         { /* lot 10 */
 147                 {0, 3, 135},
 148                 {0, 2, 90},
 149                 {0, 1, 90},
 150                 {0, 0, 90},
 151                 {1, 1, -45},
 152                 {1, 2, -90}, // FIXME: test last two pucks
 153         },
 154 };
 155
 156 /************************************************************************
 157  * NOTES ON DATA WE NEED TO STORE
 158  ************************************************************************/
 159
 160 /*
 161 PLAYGROUND:
 162  - puck (column element) dispensers status (number of pucks)
 163         - their max. capacity is 5
 164  - lintel dispensers status
 165  - free column elements configuration and their positions
 166  - free pucks configuration (lot number)
 167 ROBOT:
 168  - puck stack status (puck count)
 169  - lintel holder status
 170  */
 171
 172 /************************************************************************
 173  * MISC FUNCTIONS
 174  ************************************************************************/
 175
 176 /**
 177  * Competition timer. Stop robot when the timer exceeds.
 178  *
 179  */
 180 void *timing_thread(void *arg)
 181 {
 182         struct timespec start;
 183
 184         clock_gettime(CLOCK_MONOTONIC, &start);
 185 #define WAIT(sec)                                                       \
 186         do {                                                            \
 187                 struct timespec t;                                      \
 188                 t.tv_sec = start.tv_sec+sec;                            \
 189                 t.tv_nsec = start.tv_nsec;                              \
 190                 clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); \
 191         } while(0)
 192
 193         WAIT(5);
 194         robot.use_back_switch = true;
 195         printf("Back switch not ignored\n");
 196
 197         WAIT(TIME_TO_DEPOSITE);
 198         FSM_SIGNAL(MAIN, EV_SHORT_TIME_TO_END, NULL);
 199
 200         WAIT(COMPETITION_TIME);
 201         printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
 202         robot_exit();
 203
 204         return NULL;
 205 }
 206
 207 /**
 208  * Get position of the point when we know the distance and angle to turn.
 209  *
 210  * FIXME (F.J.): - there used to be non-actual parameter documentation
 211  *               - what was this function good for? (not used anywhere)
 212  */
 213 void get_relative_pos(struct est_pos_type *est, struct ref_pos_type *ref,
 214                         double l, double phi)
 215 {
 216         ref->x = est->x + l*cos(est->phi + phi);
 217         ref->y = est->y + l*sin(est->phi + phi);
 218         ref->phi = est->phi + phi;
 219 }
 220
 221 void robot_goto_point(struct ref_pos_type des_pos)
 222 {
 223         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 224
 225         tc.maxv /= 4;
 226         robot_trajectory_new(&tc);
 227         robot_trajectory_add_final_point_trans(des_pos.x, des_pos.y, NO_TURN());
 228 }
 229
 230 void robot_go_backward_to_point(struct ref_pos_type des_pos)
 231 {
 232         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 233
 234         tc.maxv /= 1;
 235         robot_trajectory_new_backward(&tc);
 236         robot_trajectory_add_final_point_notrans(des_pos.x, des_pos.y, NO_TURN());
 237 }
 238
 239 void robot_goto_puck_in_grid(int nx, int ny, int phi)
 240 {
 241         struct puck_pos pp = free_puck_pos(nx, ny); // puck position
 242         robot_trajectory_new(&tcSlow);
 243         robot_trajectory_add_point_trans(
 244                 pp.x + (ROBOT_AXIS_TO_PUCK_M+0.10)*cos(DEG2RAD(phi)),
 245                 pp.y + (ROBOT_AXIS_TO_PUCK_M+0.10)*sin(DEG2RAD(phi)));
 246         robot_trajectory_add_final_point_trans(
 247                 pp.x + ROBOT_AXIS_TO_PUCK_M*cos(DEG2RAD(phi)),
 248                 pp.y + ROBOT_AXIS_TO_PUCK_M*sin(DEG2RAD(phi)),
 249                 TURN(fmod(DEG2RAD(phi+180), 2.0*M_PI)));
 250         /* robot_goto_trans( // does not exist
 251                 pp.x + ROBOT_AXIS_TO_PUCK_M*cos(DEG2RAD(phi)),
 252                 pp.y + ROBOT_AXIS_TO_PUCK_M*sin(DEG2RAD(phi)),
 253                 TURN(fmod(DEG2RAD(phi+180), 2.0*M_PI))); */
 254 }
 255
 256 /************************************************************************
 257  * FSM STATES DECLARATION
 258  ************************************************************************/
 259
 260 /* initial and starting states */
 261 FSM_STATE_DECL(init);
 262 FSM_STATE_DECL(wait_for_start);
 263 /* strategies related states */
 264 FSM_STATE_DECL(collect_free_pucks);
 265 /* movement states */
 266 FSM_STATE_DECL(simple_construction_zone_approach);
 267 FSM_STATE_DECL(approach_our_static_dispenser);
 268 FSM_STATE_DECL(approach_opponents_static_dispenser);
 269 /* States handling ACT's actions (SUBFSMs) */
 270 FSM_STATE_DECL(load_the_puck);
 271 FSM_STATE_DECL(grasp_the_puck);
 272 FSM_STATE_DECL(look_for_puck_ahead);
 273 FSM_STATE_DECL(look_around_for_puck);
 274
 275 /************************************************************************
 276  * INITIAL AND STARTING STATES
 277  ************************************************************************/
 278
 279 FSM_STATE(init)
 280 {
 281         switch (FSM_EVENT) {
 282         case EV_ENTRY:
 283                 tcFast = trajectoryConstraintsDefault;
 284                 //tcFast.maxv = 1.5;
 285                 tcSlow = trajectoryConstraintsDefault;
 286                 tcSlow.maxv = 0.2;
 287                 FSM_TRANSITION(wait_for_start);
 288                 break;
 289         default:
 290                 break;
 291         }
 292 }
 293
 294 FSM_STATE(wait_for_start)
 295 {
 296         pthread_t thid;
 297 #ifdef COMPETITON
 298         printf("COMPETITION mode set");
 299 #endif
 300         switch (FSM_EVENT) {
 301 #ifdef WAIT_FOR_START
 302                 case EV_ENTRY:
 303                         break;
 304                 case EV_START:
 305 #else
 306                 case EV_ENTRY:
 307                 case EV_START:
 308 #endif
 309                         /* start competition timer */
 310                         pthread_create(&thid, NULL, timing_thread, NULL);
 311                         robot_set_est_pos_trans(0.16,
 312                                                 PLAYGROUND_HEIGHT_M - 0.16,
 313                                                 DEG2RAD(-45));
 314
 315                         FSM_TRANSITION(collect_free_pucks);
 316
 317                         /*robot_trajectory_new(&tcSlow);
 318                           struct puck_pos pp = free_puck_pos(0, 2, 0); // puck position
 319                           robot_trajectory_add_final_point_trans(
 320                           pp.x,
 321                           pp.y,
 322                           TURN(DEG2RAD(0))); */
 323                         //FSM_TRANSITION(approach_our_static_dispenser);
 324                         //robot_goto_puck_in_grid(0, 1, 2, 270);
 325                         /* temporary:
 326                            FSM_SIGNAL(ACT, EV_GRASP_THE_PUCK, NULL);
 327                            FSM_TIMER(5000);
 328                         */
 329                         break;
 330                 case EV_TIMER:
 331                         /* temporary:
 332                            FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
 333                            break;
 334                         */
 335                         break;
 336                 case EV_RETURN:
 337                 case EV_LASER_POWER:
 338                 case EV_GOAL_NOT_REACHABLE:
 339                 case EV_SHORT_TIME_TO_END:
 340                 case EV_STACK_FULL:
 341                 case EV_ACTION_DONE:
 342                 case EV_ACTION_ERROR:
 343                 case EV_PUCK_REACHABLE:
 344                 case EV_MOTION_DONE:
 345                         DBG_PRINT_EVENT("unhandled event");
 346                         break;
 347                 case EV_EXIT:
 348                         break;
 349         }
 350 }
 351
 352 /************************************************************************
 353  * STRATEGIES RELATED STATES
 354  ************************************************************************/
 355
 356 static void robot_goto_next_puck_in_sequence(int lot, int puck_number)
 357 {
 358         robot_goto_puck_in_grid(
 359                 free_puck_pick_up_sequence[lot][puck_number][0],
 360                 free_puck_pick_up_sequence[lot][puck_number][1],
 361                 free_puck_pick_up_sequence[lot][puck_number][2]);
 362 }
 363
 364 FSM_STATE(collect_free_pucks)
 365 {
 366         static const int lot = 7;  // this variable location is temporary...; going to be received from the camera
 367         switch (FSM_EVENT) {
 368                 case EV_ENTRY:
 369                         robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
 370                         break;
 371                 case EV_MOTION_DONE: {
 372                                 printf("-----arrived where the free puck no. %d should be\n", free_puck_to_try_to_get_next);
 373                                 SUBFSM_TRANSITION(load_the_puck, NULL);
 374                         }
 375                         break;
 376                 case EV_RETURN:
 377                         switch(FSM_EVENT_RET_VAL) {
 378                         /* obsolete, no sharp sensor present
 379                         case LOOK_AROUND_SUCCESS: // FIXME: test number of pucks loaded!!
 380                                 printf(">>>>>> Look around succeeded\n");
 381                                 SUBFSM_TRANSITION(load_the_puck, NULL);
 382                                 break; */
 383                         case LOAD_SUCCESS:
 384                                 printf(">>>>>> Loading the puck succeeded\n");
 385                                 if(free_puck_to_try_to_get_next<4) {
 386                                         free_puck_to_try_to_get_next++;
 387                                         robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
 388                                 }
 389                                 break;
 390                         /* obsolete, no sharp sensor present
 391                         case LOOK_AROUND_FAIL:
 392                                 printf(">>>>>> Looking around for the puck FAILED\n");
 393                                 break; // FIXME: remove the break
 394                         */
 395                         case LOAD_FAIL:
 396                                 printf(">>>>>> Loading the puck FAILED\n");
 397                                 if(free_puck_to_try_to_get_next<6) { // FIXME: test number of pucks loaded!!
 398                                         free_puck_to_try_to_get_next++;
 399                                         robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
 400                                 } else {
 401                                         // FIXME (TODO): transition to next strategy state
 402                                 }
 403                                 break;
 404                         }
 405                         break;
 406                 case EV_PUCK_REACHABLE:
 407                         robot_stop();
 408                         printf("-----reached some free puck, hopefully no. %d\n", free_puck_to_try_to_get_next);
 409                         SUBFSM_TRANSITION(load_the_puck, NULL);
 410                         break;
 411                 case EV_ACTION_DONE:
 412                 case EV_TIMER:
 413                 case EV_LASER_POWER:
 414                 case EV_GOAL_NOT_REACHABLE:
 415                 case EV_SHORT_TIME_TO_END:
 416                 case EV_STACK_FULL:
 417                 case EV_ACTION_ERROR:
 418                 case EV_START:
 419                         DBG_PRINT_EVENT("unhandled event");
 420                         break;
 421                 case EV_EXIT:
 422                         break;
 423         }
 424 }
 425
 426 /************************************************************************
 427  * MOVEMENT STATES
 428  ************************************************************************/
 429
 430 FSM_STATE(simple_construction_zone_approach)
 431 {
 432         switch (FSM_EVENT) {
 433                 case EV_ENTRY:
 434                         robot_trajectory_new(&tcFast);
 435                         robot_trajectory_add_point_trans(0.9, 1);
 436                         robot_trajectory_add_final_point_trans(0.9, ROBOT_AXIS_TO_FRONT_M + 0.05, NO_TURN());
 437                         break;
 438                 case EV_MOTION_DONE:
 439                         //FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
 440                         break;
 441                 case EV_ACTION_DONE:
 442                 case EV_RETURN:
 443                 case EV_TIMER:
 444                 case EV_LASER_POWER:
 445                 case EV_GOAL_NOT_REACHABLE:
 446                 case EV_SHORT_TIME_TO_END:
 447                 case EV_STACK_FULL:
 448                 case EV_ACTION_ERROR:
 449                 case EV_PUCK_REACHABLE:
 450                 case EV_START:
 451                         DBG_PRINT_EVENT("unhandled event");
 452                         break;
 453                 case EV_EXIT:
 454                         break;
 455         }
 456 }
 457
 458 FSM_STATE(approach_our_static_dispenser)
 459 {
 460         switch (FSM_EVENT) {
 461                 case EV_ENTRY:  {
 462                         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 463                         tc.maxv /= 1.0;
 464                         robot_trajectory_new(&tc);
 465
 466                         robot_trajectory_add_point_trans(STATIC_DISPENSER_X,
 467                                                          STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
 468                         robot_trajectory_add_final_point_trans(STATIC_DISPENSER_X,
 469                                                                STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
 470                                                                NO_TURN());
 471                         }
 472                         break;
 473                 case EV_MOTION_DONE:
 474                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 475                         printf("Arrived to the static dispenser\n");
 476                         break;
 477                 case EV_ACTION_DONE:
 478                         printf("A Puck picked up\n");
 479                         break;
 480                 case EV_RETURN:
 481                 case EV_TIMER:
 482                 case EV_LASER_POWER:
 483                 case EV_GOAL_NOT_REACHABLE:
 484                 case EV_SHORT_TIME_TO_END:
 485                 //case EV_PUCK_REACHABLE: // FIXME: handle this
 486                 case EV_STACK_FULL:
 487                 case EV_ACTION_ERROR:
 488                 case EV_PUCK_REACHABLE:
 489                 case EV_START:
 490                         DBG_PRINT_EVENT("unhandled event");
 491                         break;
 492                 case EV_EXIT:
 493                         break;
 494         }
 495 }
 496
 497 FSM_STATE(approach_opponents_static_dispenser)
 498 {
 499         switch (FSM_EVENT) {
 500                 case EV_ENTRY:  {
 501                         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 502                         tc.maxv /= 1.0;
 503                         robot_trajectory_new(&tc);
 504
 505                         robot_trajectory_add_point_trans(OPPONENTS_STATIC_DISPENSER_X,
 506                                                          OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
 507                         robot_trajectory_add_final_point_trans(OPPONENTS_STATIC_DISPENSER_X,
 508                                                                OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
 509                                                                NO_TURN());
 510                         }
 511                         break;
 512                 case EV_MOTION_DONE:
 513                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 514                         printf("Arrived to the static dispenser\n");
 515                         break;
 516                 case EV_ACTION_DONE:
 517                         printf("A Puck picked up\n");
 518                         break;
 519                 case EV_RETURN:
 520                 case EV_TIMER:
 521                 case EV_LASER_POWER:
 522                 case EV_GOAL_NOT_REACHABLE:
 523                 case EV_SHORT_TIME_TO_END:
 524                 //case EV_PUCK_REACHABLE: // FIXME: handle this
 525                 case EV_STACK_FULL:
 526                 case EV_ACTION_ERROR:
 527                 case EV_PUCK_REACHABLE:
 528                 case EV_START:
 529                         DBG_PRINT_EVENT("unhandled event");
 530                         break;
 531                 case EV_EXIT:
 532                         break;
 533         }
 534 }
 535
 536 /************************************************************************
 537  * STATES HANDLING ACT's ACTIONS (to be used as SUB FSMs)
 538  ************************************************************************/
 539
 540 FSM_STATE(load_the_puck)
 541 {
 542         static int puck_load_attempt_count;
 543         switch (FSM_EVENT) {
 544                 case EV_ENTRY:
 545                         puck_load_attempt_count = 0;
 546                         FSM_SIGNAL(ACT, EV_SCRABBLE, NULL);
 547                         FSM_TIMER(200);
 548                         break;
 549                 case EV_TIMER:
 550                         robot_move_by(0.02, NO_TURN(), &tcSlow);
 551                         break;
 552                 case EV_MOTION_DONE:
 553                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 554                         break;
 555                 case EV_ACTION_DONE:
 556                         SUBFSM_RET((void *)LOAD_SUCCESS);
 557                         break;
 558                 case EV_ACTION_ERROR:
 559                         puck_load_attempt_count++;
 560                         if (puck_load_attempt_count > 2) {
 561                                 SUBFSM_RET((void *)LOAD_FAIL);
 562                         } else {
 563                                 robot_move_by(0.02, NO_TURN(), &tcSlow);
 564                         }
 565                         break;
 566                 case EV_RETURN:
 567                 case EV_LASER_POWER:
 568                 case EV_GOAL_NOT_REACHABLE:
 569                 case EV_SHORT_TIME_TO_END:
 570                 case EV_STACK_FULL:
 571                 case EV_PUCK_REACHABLE:
 572                 case EV_START:
 573                         DBG_PRINT_EVENT("unhandled event");
 574                         break;
 575                 case EV_EXIT:
 576                         break;
 577         }
 578 }
 579
 580 /* of no use without the sharp sensor measuring "puck distance"
 581 FSM_STATE(look_around_for_puck)
 582 {
 583         static int lfp_status = 0;
 584         const static int scatter_angle = 20;
 585         static struct ref_pos_type orig_position;
 586         switch (FSM_EVENT) {
 587                 case EV_ENTRY:
 588                         ROBOT_LOCK(ref_pos);
 589                         orig_position = robot.ref_pos;
 590                         ROBOT_UNLOCK(ref_pos);
 591                         //printf("original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 592                         robot_move_by(0, TURN_CW(DEG2RAD(RAD2DEG(orig_position.phi)-scatter_angle)), &tcSlow);
 593                         //printf("lfp_status: %d\n", lfp_status);
 594                         break;
 595                 case EV_MOTION_DONE:
 596                         switch (lfp_status) {
 597                         case 0:
 598                                 //printf("2. original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 599                                 //printf("robot.fer_pos angle of rotation of the robot: %f degrees\n", RAD2DEG(robot.ref_pos.phi));
 600                                 //printf("--- robot move by ... turn cw\n");
 601                                 robot_move_by(0, TURN_CCW(DEG2RAD(RAD2DEG(orig_position.phi)+scatter_angle)), &tcSlow);
 602                                 lfp_status++;
 603                                 break;
 604                         case 1:
 605                                 robot_move_by(0, TURN(orig_position.phi), &tcSlow);
 606                                 lfp_status++;
 607                                 break;
 608                         case 2: // puck not found
 609                                 SUBFSM_RET((void *)LOOK_AROUND_FAIL);
 610                                 break;
 611                         }
 612                         //printf("lfp_status: %d\n", lfp_status);
 613                         break;
 614                 case EV_PUCK_REACHABLE: // puck found
 615                         robot_stop();
 616                         SUBFSM_RET((void *)LOOK_AROUND_SUCCESS);
 617                         break;
 618                 case EV_ACTION_DONE:
 619                 case EV_ACTION_ERROR: // look for puck does not send this event
 620                 case EV_RETURN:
 621                 case EV_TIMER:
 622                 case EV_LASER_POWER:
 623                 case EV_GOAL_NOT_REACHABLE:
 624                 case EV_SHORT_TIME_TO_END:
 625                 case EV_STACK_FULL:
 626                 case EV_START:
 627                         DBG_PRINT_EVENT("unhandled event");
 628                         break;
 629                 case EV_EXIT:
 630                         break;
 631         }
 632 }
 633 */
 634
 635
 636 /*
 637 FSM_STATE()
 638 {
 639         switch (FSM_EVENT) {
 640                 case EV_ENTRY:
 641                         break;
 642                 case EV_MOTION_DONE:
 643                 case EV_ACTION_DONE:
 644                 case EV_RETURN:
 645                 case EV_TIMER:
 646                 case EV_OBSTRUCTION_AHEAD:
 647                 case EV_LASER_POWER:
 648                 case EV_GOAL_NOT_REACHABLE:
 649                 case EV_SHORT_TIME_TO_END:
 650                 case EV_ENEMY_AHEAD:
 651                 case EV_STACK_FULL:
 652                 case EV_ACTION_ERROR:
 653                 case EV_PUCK_REACHABLE:
 654                 case EV_START:
 655                         DBG_PRINT_EVENT("unhandled event");
 656                         break;
 657                 case EV_EXIT:
 658                         break;
 659         }
 660 }
 661 */
 662