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 //      static const int lot = robot.game_conf; // +1?? game_conf is 0..9
 369         switch (FSM_EVENT) {
 370                 case EV_ENTRY:
 371                         robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
 372                         break;
 373                 case EV_MOTION_DONE: {
 374                                 printf("-----arrived where the free puck no. %d should be\n", free_puck_to_try_to_get_next);
 375                                 SUBFSM_TRANSITION(load_the_puck, NULL);
 376                         }
 377                         break;
 378                 case EV_RETURN:
 379                         switch(FSM_EVENT_RET_VAL) {
 380                         /* obsolete, no sharp sensor present
 381                         case LOOK_AROUND_SUCCESS: // FIXME: test number of pucks loaded!!
 382                                 printf(">>>>>> Look around succeeded\n");
 383                                 SUBFSM_TRANSITION(load_the_puck, NULL);
 384                                 break; */
 385                         case LOAD_SUCCESS:
 386                                 printf(">>>>>> Loading the puck succeeded\n");
 387                                 if(free_puck_to_try_to_get_next<4) {
 388                                         free_puck_to_try_to_get_next++;
 389                                         robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
 390                                 }
 391                                 break;
 392                         /* obsolete, no sharp sensor present
 393                         case LOOK_AROUND_FAIL:
 394                                 printf(">>>>>> Looking around for the puck FAILED\n");
 395                                 break; // FIXME: remove the break
 396                         */
 397                         case LOAD_FAIL:
 398                                 printf(">>>>>> Loading the puck FAILED\n");
 399                                 if(free_puck_to_try_to_get_next<6) { // FIXME: test number of pucks loaded!!
 400                                         free_puck_to_try_to_get_next++;
 401                                         robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
 402                                 } else {
 403                                         // FIXME (TODO): transition to next strategy state
 404                                 }
 405                                 break;
 406                         }
 407                         break;
 408                 case EV_PUCK_REACHABLE:
 409                         robot_stop();
 410                         printf("-----reached some free puck, hopefully no. %d\n", free_puck_to_try_to_get_next);
 411                         SUBFSM_TRANSITION(load_the_puck, NULL);
 412                         break;
 413                 case EV_ACTION_DONE:
 414                 case EV_TIMER:
 415                 case EV_LASER_POWER:
 416                 case EV_GOAL_NOT_REACHABLE:
 417                 case EV_SHORT_TIME_TO_END:
 418                 case EV_STACK_FULL:
 419                 case EV_ACTION_ERROR:
 420                 case EV_START:
 421                         DBG_PRINT_EVENT("unhandled event");
 422                         break;
 423                 case EV_EXIT:
 424                         break;
 425         }
 426 }
 427
 428 /************************************************************************
 429  * MOVEMENT STATES
 430  ************************************************************************/
 431
 432 FSM_STATE(simple_construction_zone_approach)
 433 {
 434         switch (FSM_EVENT) {
 435                 case EV_ENTRY:
 436                         robot_trajectory_new(&tcFast);
 437                         robot_trajectory_add_point_trans(0.9, 1);
 438                         robot_trajectory_add_final_point_trans(0.9, ROBOT_AXIS_TO_FRONT_M + 0.05, NO_TURN());
 439                         break;
 440                 case EV_MOTION_DONE:
 441                         //FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
 442                         break;
 443                 case EV_ACTION_DONE:
 444                 case EV_RETURN:
 445                 case EV_TIMER:
 446                 case EV_LASER_POWER:
 447                 case EV_GOAL_NOT_REACHABLE:
 448                 case EV_SHORT_TIME_TO_END:
 449                 case EV_STACK_FULL:
 450                 case EV_ACTION_ERROR:
 451                 case EV_PUCK_REACHABLE:
 452                 case EV_START:
 453                         DBG_PRINT_EVENT("unhandled event");
 454                         break;
 455                 case EV_EXIT:
 456                         break;
 457         }
 458 }
 459
 460 FSM_STATE(approach_our_static_dispenser)
 461 {
 462         switch (FSM_EVENT) {
 463                 case EV_ENTRY:  {
 464                         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 465                         tc.maxv /= 1.0;
 466                         robot_trajectory_new(&tc);
 467
 468                         robot_trajectory_add_point_trans(STATIC_DISPENSER_X,
 469                                                          STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
 470                         robot_trajectory_add_final_point_trans(STATIC_DISPENSER_X,
 471                                                                STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
 472                                                                NO_TURN());
 473                         }
 474                         break;
 475                 case EV_MOTION_DONE:
 476                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 477                         printf("Arrived to the static dispenser\n");
 478                         break;
 479                 case EV_ACTION_DONE:
 480                         printf("A Puck picked up\n");
 481                         break;
 482                 case EV_RETURN:
 483                 case EV_TIMER:
 484                 case EV_LASER_POWER:
 485                 case EV_GOAL_NOT_REACHABLE:
 486                 case EV_SHORT_TIME_TO_END:
 487                 //case EV_PUCK_REACHABLE: // FIXME: handle this
 488                 case EV_STACK_FULL:
 489                 case EV_ACTION_ERROR:
 490                 case EV_PUCK_REACHABLE:
 491                 case EV_START:
 492                         DBG_PRINT_EVENT("unhandled event");
 493                         break;
 494                 case EV_EXIT:
 495                         break;
 496         }
 497 }
 498
 499 FSM_STATE(approach_opponents_static_dispenser)
 500 {
 501         switch (FSM_EVENT) {
 502                 case EV_ENTRY:  {
 503                         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 504                         tc.maxv /= 1.0;
 505                         robot_trajectory_new(&tc);
 506
 507                         robot_trajectory_add_point_trans(OPPONENTS_STATIC_DISPENSER_X,
 508                                                          OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
 509                         robot_trajectory_add_final_point_trans(OPPONENTS_STATIC_DISPENSER_X,
 510                                                                OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
 511                                                                NO_TURN());
 512                         }
 513                         break;
 514                 case EV_MOTION_DONE:
 515                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 516                         printf("Arrived to the static dispenser\n");
 517                         break;
 518                 case EV_ACTION_DONE:
 519                         printf("A Puck picked up\n");
 520                         break;
 521                 case EV_RETURN:
 522                 case EV_TIMER:
 523                 case EV_LASER_POWER:
 524                 case EV_GOAL_NOT_REACHABLE:
 525                 case EV_SHORT_TIME_TO_END:
 526                 //case EV_PUCK_REACHABLE: // FIXME: handle this
 527                 case EV_STACK_FULL:
 528                 case EV_ACTION_ERROR:
 529                 case EV_PUCK_REACHABLE:
 530                 case EV_START:
 531                         DBG_PRINT_EVENT("unhandled event");
 532                         break;
 533                 case EV_EXIT:
 534                         break;
 535         }
 536 }
 537
 538 /************************************************************************
 539  * STATES HANDLING ACT's ACTIONS (to be used as SUB FSMs)
 540  ************************************************************************/
 541
 542 FSM_STATE(load_the_puck)
 543 {
 544         static int puck_load_attempt_count;
 545         switch (FSM_EVENT) {
 546                 case EV_ENTRY:
 547                         puck_load_attempt_count = 0;
 548                         FSM_SIGNAL(ACT, EV_SCRABBLE, NULL);
 549                         FSM_TIMER(200);
 550                         break;
 551                 case EV_TIMER:
 552                         robot_move_by(0.02, NO_TURN(), &tcSlow);
 553                         break;
 554                 case EV_MOTION_DONE:
 555                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 556                         break;
 557                 case EV_ACTION_DONE:
 558                         SUBFSM_RET((void *)LOAD_SUCCESS);
 559                         break;
 560                 case EV_ACTION_ERROR:
 561                         puck_load_attempt_count++;
 562                         if (puck_load_attempt_count > 2) {
 563                                 SUBFSM_RET((void *)LOAD_FAIL);
 564                         } else {
 565                                 robot_move_by(0.02, NO_TURN(), &tcSlow);
 566                         }
 567                         break;
 568                 case EV_RETURN:
 569                 case EV_LASER_POWER:
 570                 case EV_GOAL_NOT_REACHABLE:
 571                 case EV_SHORT_TIME_TO_END:
 572                 case EV_STACK_FULL:
 573                 case EV_PUCK_REACHABLE:
 574                 case EV_START:
 575                         DBG_PRINT_EVENT("unhandled event");
 576                         break;
 577                 case EV_EXIT:
 578                         break;
 579         }
 580 }
 581
 582 /* of no use without the sharp sensor measuring "puck distance"
 583 FSM_STATE(look_around_for_puck)
 584 {
 585         static int lfp_status = 0;
 586         const static int scatter_angle = 20;
 587         static struct ref_pos_type orig_position;
 588         switch (FSM_EVENT) {
 589                 case EV_ENTRY:
 590                         ROBOT_LOCK(ref_pos);
 591                         orig_position = robot.ref_pos;
 592                         ROBOT_UNLOCK(ref_pos);
 593                         //printf("original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 594                         robot_move_by(0, TURN_CW(DEG2RAD(RAD2DEG(orig_position.phi)-scatter_angle)), &tcSlow);
 595                         //printf("lfp_status: %d\n", lfp_status);
 596                         break;
 597                 case EV_MOTION_DONE:
 598                         switch (lfp_status) {
 599                         case 0:
 600                                 //printf("2. original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 601                                 //printf("robot.fer_pos angle of rotation of the robot: %f degrees\n", RAD2DEG(robot.ref_pos.phi));
 602                                 //printf("--- robot move by ... turn cw\n");
 603                                 robot_move_by(0, TURN_CCW(DEG2RAD(RAD2DEG(orig_position.phi)+scatter_angle)), &tcSlow);
 604                                 lfp_status++;
 605                                 break;
 606                         case 1:
 607                                 robot_move_by(0, TURN(orig_position.phi), &tcSlow);
 608                                 lfp_status++;
 609                                 break;
 610                         case 2: // puck not found
 611                                 SUBFSM_RET((void *)LOOK_AROUND_FAIL);
 612                                 break;
 613                         }
 614                         //printf("lfp_status: %d\n", lfp_status);
 615                         break;
 616                 case EV_PUCK_REACHABLE: // puck found
 617                         robot_stop();
 618                         SUBFSM_RET((void *)LOOK_AROUND_SUCCESS);
 619                         break;
 620                 case EV_ACTION_DONE:
 621                 case EV_ACTION_ERROR: // look for puck does not send this event
 622                 case EV_RETURN:
 623                 case EV_TIMER:
 624                 case EV_LASER_POWER:
 625                 case EV_GOAL_NOT_REACHABLE:
 626                 case EV_SHORT_TIME_TO_END:
 627                 case EV_STACK_FULL:
 628                 case EV_START:
 629                         DBG_PRINT_EVENT("unhandled event");
 630                         break;
 631                 case EV_EXIT:
 632                         break;
 633         }
 634 }
 635 */
 636
 637
 638 /*
 639 FSM_STATE()
 640 {
 641         switch (FSM_EVENT) {
 642                 case EV_ENTRY:
 643                         break;
 644                 case EV_MOTION_DONE:
 645                 case EV_ACTION_DONE:
 646                 case EV_RETURN:
 647                 case EV_TIMER:
 648                 case EV_OBSTRUCTION_AHEAD:
 649                 case EV_LASER_POWER:
 650                 case EV_GOAL_NOT_REACHABLE:
 651                 case EV_SHORT_TIME_TO_END:
 652                 case EV_ENEMY_AHEAD:
 653                 case EV_STACK_FULL:
 654                 case EV_ACTION_ERROR:
 655                 case EV_PUCK_REACHABLE:
 656                 case EV_START:
 657                         DBG_PRINT_EVENT("unhandled event");
 658                         break;
 659                 case EV_EXIT:
 660                         break;
 661         }
 662 }
 663 */
 664
 665
 666 int main()
 667 {
 668         int rv;
 669
 670         rv = robot_init();
 671         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 672
 673         robot.fsm.main.debug_states = 1;
 674         robot.fsm.motion.debug_states = 1;
 675         robot.fsm.act.debug_states = 1;
 676
 677         robot.fsm.main.state = &fsm_state_main_init;
 678
 679         rv = robot_start();
 680         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 681
 682         robot_destroy();
 683
 684         return 0;
 685 }