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