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