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