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 } subfsm_ret_val;
  53
  54 #define FSM_EVENT_RET_VAL ((subfsm_ret_val)FSM_EVENT_INT)
  55
  56 /************************************************************************
  57  * Trajectory constraints used, are initialized in the init state
  58  ************************************************************************/
  59
  60 struct TrajectoryConstraints tcFast, tcSlow;
  61
  62 /************************************************************************
  63  * Variables related to puck collecting
  64  ************************************************************************/
  65
  66 int free_puck_to_try_to_get_next; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
  67 int pucks_at_once; // number of pucks to load at once (maximum number of pucks we want to have in robot)
  68
  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                 free_puck_to_try_to_get_next = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
 285                 pucks_at_once = 2; // number of pucks to load at once (maximum number of pucks we want to have in robot)
 286                 tcFast = trajectoryConstraintsDefault;
 287                 //tcFast.maxv = 1.5;
 288                 tcSlow = trajectoryConstraintsDefault;
 289                 tcSlow.maxv = 0.2;
 290                 FSM_TRANSITION(wait_for_start);
 291                 break;
 292         default:
 293                 break;
 294         }
 295 }
 296
 297 FSM_STATE(wait_for_start)
 298 {
 299         pthread_t thid;
 300 #ifdef COMPETITON
 301         printf("COMPETITION mode set");
 302 #endif
 303         switch (FSM_EVENT) {
 304 #ifdef WAIT_FOR_START
 305                 case EV_ENTRY:
 306                         break;
 307                 case EV_START:
 308 #else
 309                 case EV_ENTRY:
 310                 case EV_START:
 311 #endif
 312                         /* start competition timer */
 313                         pthread_create(&thid, NULL, timing_thread, NULL);
 314                         robot_set_est_pos_trans(0.16,
 315                                                 PLAYGROUND_HEIGHT_M - 0.16,
 316                                                 DEG2RAD(-45));
 317
 318                         FSM_TRANSITION(collect_free_pucks);
 319
 320                         /*robot_trajectory_new(&tcSlow);
 321                           struct puck_pos pp = free_puck_pos(0, 2, 0); // puck position
 322                           robot_trajectory_add_final_point_trans(
 323                           pp.x,
 324                           pp.y,
 325                           TURN(DEG2RAD(0))); */
 326                         //FSM_TRANSITION(approach_our_static_dispenser);
 327                         //robot_goto_puck_in_grid(0, 1, 2, 270);
 328                         /* temporary:
 329                            FSM_SIGNAL(ACT, EV_GRASP_THE_PUCK, NULL);
 330                            FSM_TIMER(5000);
 331                         */
 332                         break;
 333                 case EV_TIMER:
 334                         /* temporary:
 335                            FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
 336                            break;
 337                         */
 338                         break;
 339                 case EV_RETURN:
 340                 case EV_LASER_POWER:
 341                 case EV_GOAL_NOT_REACHABLE:
 342                 case EV_SHORT_TIME_TO_END:
 343                 case EV_STACK_FULL:
 344                 case EV_ACTION_DONE:
 345                 case EV_ACTION_ERROR:
 346                 case EV_PUCK_REACHABLE:
 347                 case EV_MOTION_DONE:
 348                         DBG_PRINT_EVENT("unhandled event");
 349                         break;
 350                 case EV_EXIT:
 351                         break;
 352         }
 353 }
 354
 355 /************************************************************************
 356  * STRATEGIES RELATED STATES
 357  ************************************************************************/
 358
 359 static void robot_goto_next_puck_in_sequence(int lot, int puck_number)
 360 {
 361         robot_goto_puck_in_grid(
 362                 free_puck_pick_up_sequence[lot][puck_number][0],
 363                 free_puck_pick_up_sequence[lot][puck_number][1],
 364                 free_puck_pick_up_sequence[lot][puck_number][2]);
 365 }
 366
 367 FSM_STATE(collect_free_pucks)
 368 {
 369         static const int lot = 7;  // this variable location is temporary...; going to be received from the camera
 370         switch (FSM_EVENT) {
 371                 case EV_ENTRY:
 372                         robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
 373                         break;
 374                 case EV_MOTION_DONE: {
 375                                 printf("-----arrived where the free puck no. %d should be\n", free_puck_to_try_to_get_next);
 376                                 SUBFSM_TRANSITION(load_the_puck, NULL);
 377                         }
 378                         break;
 379                 case EV_RETURN:
 380                         switch(FSM_EVENT_RET_VAL) {
 381                         case LOAD_SUCCESS:
 382                                 printf(">>>>>> Loading the puck succeeded\n");
 383                                 if(free_puck_to_try_to_get_next<4) {
 384                                         free_puck_to_try_to_get_next++;
 385                                         robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
 386                                 }
 387                                 break;
 388                         case LOAD_FAIL:
 389                                 printf(">>>>>> Loading the puck FAILED\n");
 390                                 if(free_puck_to_try_to_get_next<6) { // FIXME: test number of pucks loaded!!
 391                                         free_puck_to_try_to_get_next++;
 392                                         robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
 393                                 } else {
 394                                         // FIXME (TODO): transition to next strategy state
 395                                 }
 396                                 break;
 397                         }
 398                         break;
 399                 case EV_PUCK_REACHABLE:
 400                         robot_stop();
 401                         printf("-----reached some free puck, hopefully no. %d\n", free_puck_to_try_to_get_next);
 402                         SUBFSM_TRANSITION(load_the_puck, NULL);
 403                         break;
 404                 case EV_ACTION_DONE:
 405                 case EV_TIMER:
 406                 case EV_LASER_POWER:
 407                 case EV_GOAL_NOT_REACHABLE:
 408                 case EV_SHORT_TIME_TO_END:
 409                 case EV_STACK_FULL:
 410                 case EV_ACTION_ERROR:
 411                 case EV_START:
 412                         DBG_PRINT_EVENT("unhandled event");
 413                         break;
 414                 case EV_EXIT:
 415                         break;
 416         }
 417 }
 418
 419 /************************************************************************
 420  * MOVEMENT STATES
 421  ************************************************************************/
 422
 423 FSM_STATE(simple_construction_zone_approach)
 424 {
 425         switch (FSM_EVENT) {
 426                 case EV_ENTRY:
 427                         robot_trajectory_new(&tcFast);
 428                         robot_trajectory_add_point_trans(0.9, 1);
 429                         robot_trajectory_add_final_point_trans(0.9, ROBOT_AXIS_TO_FRONT_M + 0.05, NO_TURN());
 430                         break;
 431                 case EV_MOTION_DONE:
 432                         //FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
 433                         break;
 434                 case EV_ACTION_DONE:
 435                 case EV_RETURN:
 436                 case EV_TIMER:
 437                 case EV_LASER_POWER:
 438                 case EV_GOAL_NOT_REACHABLE:
 439                 case EV_SHORT_TIME_TO_END:
 440                 case EV_STACK_FULL:
 441                 case EV_ACTION_ERROR:
 442                 case EV_PUCK_REACHABLE:
 443                 case EV_START:
 444                         DBG_PRINT_EVENT("unhandled event");
 445                         break;
 446                 case EV_EXIT:
 447                         break;
 448         }
 449 }
 450
 451 FSM_STATE(approach_our_static_dispenser)
 452 {
 453         switch (FSM_EVENT) {
 454                 case EV_ENTRY:  {
 455                         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 456                         tc.maxv /= 1.0;
 457                         robot_trajectory_new(&tc);
 458
 459                         robot_trajectory_add_point_trans(STATIC_DISPENSER_X,
 460                                                          STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
 461                         robot_trajectory_add_final_point_trans(STATIC_DISPENSER_X,
 462                                                                STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
 463                                                                NO_TURN());
 464                         }
 465                         break;
 466                 case EV_MOTION_DONE:
 467                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 468                         printf("Arrived to the static dispenser\n");
 469                         break;
 470                 case EV_ACTION_DONE:
 471                         printf("A Puck picked up\n");
 472                         break;
 473                 case EV_RETURN:
 474                 case EV_TIMER:
 475                 case EV_LASER_POWER:
 476                 case EV_GOAL_NOT_REACHABLE:
 477                 case EV_SHORT_TIME_TO_END:
 478                 //case EV_PUCK_REACHABLE: // FIXME: handle this
 479                 case EV_STACK_FULL:
 480                 case EV_ACTION_ERROR:
 481                 case EV_PUCK_REACHABLE:
 482                 case EV_START:
 483                         DBG_PRINT_EVENT("unhandled event");
 484                         break;
 485                 case EV_EXIT:
 486                         break;
 487         }
 488 }
 489
 490 FSM_STATE(approach_opponents_static_dispenser)
 491 {
 492         switch (FSM_EVENT) {
 493                 case EV_ENTRY:  {
 494                         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 495                         tc.maxv /= 1.0;
 496                         robot_trajectory_new(&tc);
 497
 498                         robot_trajectory_add_point_trans(OPPONENTS_STATIC_DISPENSER_X,
 499                                                          OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
 500                         robot_trajectory_add_final_point_trans(OPPONENTS_STATIC_DISPENSER_X,
 501                                                                OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
 502                                                                NO_TURN());
 503                         }
 504                         break;
 505                 case EV_MOTION_DONE:
 506                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 507                         printf("Arrived to the static dispenser\n");
 508                         break;
 509                 case EV_ACTION_DONE:
 510                         printf("A Puck picked up\n");
 511                         break;
 512                 case EV_RETURN:
 513                 case EV_TIMER:
 514                 case EV_LASER_POWER:
 515                 case EV_GOAL_NOT_REACHABLE:
 516                 case EV_SHORT_TIME_TO_END:
 517                 //case EV_PUCK_REACHABLE: // FIXME: handle this
 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 /************************************************************************
 530  * STATES HANDLING ACT's ACTIONS (to be used as SUB FSMs)
 531  ************************************************************************/
 532
 533 FSM_STATE(load_the_puck)
 534 {
 535         static int puck_load_attempt_count;
 536         switch (FSM_EVENT) {
 537                 case EV_ENTRY:
 538                         puck_load_attempt_count = 0;
 539                         FSM_SIGNAL(ACT, EV_SCRABBLE, NULL);
 540                         FSM_TIMER(200);
 541                         break;
 542                 case EV_TIMER:
 543                         robot_move_by(0.02, NO_TURN(), &tcSlow);
 544                         break;
 545                 case EV_MOTION_DONE:
 546                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 547                         break;
 548                 case EV_ACTION_DONE:
 549                         SUBFSM_RET((void *)LOAD_SUCCESS);
 550                         break;
 551                 case EV_ACTION_ERROR:
 552                         puck_load_attempt_count++;
 553                         if (puck_load_attempt_count > 2) {
 554                                 SUBFSM_RET((void *)LOAD_FAIL);
 555                         } else {
 556                                 robot_move_by(0.02, NO_TURN(), &tcSlow);
 557                         }
 558                         break;
 559                 case EV_RETURN:
 560                 case EV_LASER_POWER:
 561                 case EV_GOAL_NOT_REACHABLE:
 562                 case EV_SHORT_TIME_TO_END:
 563                 case EV_STACK_FULL:
 564                 case EV_PUCK_REACHABLE:
 565                 case EV_START:
 566                         DBG_PRINT_EVENT("unhandled event");
 567                         break;
 568                 case EV_EXIT:
 569                         break;
 570         }
 571 }
 572
 573 /* of no use without the sharp sensor measuring "puck distance"
 574 FSM_STATE(look_around_for_puck)
 575 {
 576         static int lfp_status = 0;
 577         const static int scatter_angle = 20;
 578         static struct ref_pos_type orig_position;
 579         switch (FSM_EVENT) {
 580                 case EV_ENTRY:
 581                         ROBOT_LOCK(ref_pos);
 582                         orig_position = robot.ref_pos;
 583                         ROBOT_UNLOCK(ref_pos);
 584                         //printf("original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 585                         robot_move_by(0, TURN_CW(DEG2RAD(RAD2DEG(orig_position.phi)-scatter_angle)), &tcSlow);
 586                         //printf("lfp_status: %d\n", lfp_status);
 587                         break;
 588                 case EV_MOTION_DONE:
 589                         switch (lfp_status) {
 590                         case 0:
 591                                 //printf("2. original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 592                                 //printf("robot.fer_pos angle of rotation of the robot: %f degrees\n", RAD2DEG(robot.ref_pos.phi));
 593                                 //printf("--- robot move by ... turn cw\n");
 594                                 robot_move_by(0, TURN_CCW(DEG2RAD(RAD2DEG(orig_position.phi)+scatter_angle)), &tcSlow);
 595                                 lfp_status++;
 596                                 break;
 597                         case 1:
 598                                 robot_move_by(0, TURN(orig_position.phi), &tcSlow);
 599                                 lfp_status++;
 600                                 break;
 601                         case 2: // puck not found
 602                                 SUBFSM_RET((void *)LOOK_AROUND_FAIL);
 603                                 break;
 604                         }
 605                         //printf("lfp_status: %d\n", lfp_status);
 606                         break;
 607                 case EV_PUCK_REACHABLE: // puck found
 608                         robot_stop();
 609                         SUBFSM_RET((void *)LOOK_AROUND_SUCCESS);
 610                         break;
 611                 case EV_ACTION_DONE:
 612                 case EV_ACTION_ERROR: // look for puck does not send this event
 613                 case EV_RETURN:
 614                 case EV_TIMER:
 615                 case EV_LASER_POWER:
 616                 case EV_GOAL_NOT_REACHABLE:
 617                 case EV_SHORT_TIME_TO_END:
 618                 case EV_STACK_FULL:
 619                 case EV_START:
 620                         DBG_PRINT_EVENT("unhandled event");
 621                         break;
 622                 case EV_EXIT:
 623                         break;
 624         }
 625 }
 626 */
 627
 628
 629 /*
 630 FSM_STATE()
 631 {
 632         switch (FSM_EVENT) {
 633                 case EV_ENTRY:
 634                         break;
 635                 case EV_MOTION_DONE:
 636                 case EV_ACTION_DONE:
 637                 case EV_RETURN:
 638                 case EV_TIMER:
 639                 case EV_OBSTRUCTION_AHEAD:
 640                 case EV_LASER_POWER:
 641                 case EV_GOAL_NOT_REACHABLE:
 642                 case EV_SHORT_TIME_TO_END:
 643                 case EV_ENEMY_AHEAD:
 644                 case EV_STACK_FULL:
 645                 case EV_ACTION_ERROR:
 646                 case EV_PUCK_REACHABLE:
 647                 case EV_START:
 648                         DBG_PRINT_EVENT("unhandled event");
 649                         break;
 650                 case EV_EXIT:
 651                         break;
 652         }
 653 }
 654 */
 655
 656
 657 int main()
 658 {
 659         int rv;
 660
 661         rv = robot_init();
 662         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 663
 664         robot.fsm.main.debug_states = 1;
 665         robot.fsm.motion.debug_states = 1;
 666         robot.fsm.act.debug_states = 1;
 667
 668         robot.fsm.main.state = &fsm_state_main_init;
 669
 670         rv = robot_start();
 671         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 672
 673         robot_destroy();
 674
 675         return 0;
 676 }