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