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 #include <actuators.h>
  30
  31 #ifdef COMPETITION
  32 #define WAIT_FOR_START
  33 #define COMPETITION_TIME_DEFAULT        90
  34 #define TIME_TO_DEPOSITE_DEFAULT        60
  35 #else
  36 #undef WAIT_FOR_START
  37 #define COMPETITION_TIME_DEFAULT        900
  38 #define TIME_TO_DEPOSITE_DEFAULT        60
  39 #endif
  40
  41 /* competition time in seconds */
  42 #define COMPETITION_TIME        COMPETITION_TIME_DEFAULT
  43 #define TIME_TO_DEPOSITE        TIME_TO_DEPOSITE_DEFAULT
  44 /* competition time in seconds */
  45
  46 /************************************************************************
  47  * SUBFSM's return values ...
  48  ************************************************************************/
  49
  50 typedef enum {
  51         LOAD_SUCCESS = 0,
  52         LOAD_FAIL,
  53 } subfsm_ret_val;
  54
  55 #define FSM_EVENT_RET_VAL ((subfsm_ret_val)FSM_EVENT_INT)
  56
  57 /************************************************************************
  58  * Trajectory constraints used, are initialized in the init state
  59  ************************************************************************/
  60
  61 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
  62
  63 /************************************************************************
  64  * Variables related to puck collecting
  65  ************************************************************************/
  66
  67 int free_puck_to_try_to_get_next; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
  68 int pucks_at_once; // number of pucks to load at once (maximum number of pucks we want to have in robot)
  69
  70 /************************************************************************
  71  * Definition of particular "free pucks pick up sequences" and related stuff
  72  ************************************************************************/
  73
  74 // FIXME: has to be redefined and tested
  75 const int free_puck_pick_up_sequence[][6][3] = {
  76 /* nx, ny, arrival angle */
  77         { /* lot 1 */
  78                 {0, 3, 135},
  79                 {1, 3, 180},
  80                 {2, 3, 180},
  81                 {2, 0, 90},
  82                 {1, 0, 45},
  83                 {0, 0, 0},
  84         },
  85         {
  86                 {0, 3, 135},
  87                 {2, 3, 90},
  88                 {2, 2, 90},
  89                 {2, 1, 90},
  90                 {2, 0, 90},
  91                 {0, 0, 10},
  92         },
  93         {
  94                 {0, 3, 125},
  95                 {2, 3, 160},
  96                 {1, 2, 40},
  97                 {1, 1, 90},
  98                 {2, 0, 135},
  99                 {0, 0, 0},
 100         },
 101         { /* lot 4 */
 102                 {0, 3, 135},
 103                 {0, 2, 90},
 104                 {0, 1, 90},
 105                 {0, 0, 90},
 106                 {2, 3, -90},
 107                 {2, 0, 90},
 108         },
 109         {
 110                 {0, 3, 135},
 111                 {1, 3, 180},
 112                 {2, 2, 135},
 113                 {2, 1, 90},
 114                 {1, 0, 0},
 115                 {0, 0, 0},
 116         },
 117         { /* lot 6 */
 118                 {0, 3, 135},
 119                 {1, 3, 180},
 120                 {1, 2, 135},
 121                 {1, 1, 90},
 122                 {1, 0, 0},
 123                 {0, 0, 0},
 124         },
 125         {
 126                 {0, 3, 135},
 127                 {0, 2, 90},
 128                 {0, 1, 90},
 129                 {0, 0, 90},
 130                 {1, 3, -45},
 131                 {1, 0, 45},
 132         },
 133         { /* lot 8 */
 134                 {0, 3, 135},
 135                 {1, 2, 135},
 136                 {1, 1, 90},
 137                 {0, 0, 45},
 138                 {2, 2, -90},
 139                 {2, 1, -90},
 140         },
 141         {
 142                 {0, 3, 135},
 143                 {0, 2, 90},
 144                 {0, 1, 90},
 145                 {0, 0, 90},
 146                 {2, 2, -90},
 147                 {2, 1, -90},
 148         },
 149         { /* lot 10 */
 150                 {0, 3, 135},
 151                 {0, 2, 90},
 152                 {0, 1, 90},
 153                 {0, 0, 90},
 154                 {1, 1, -45},
 155                 {1, 2, -90},
 156         },
 157 };
 158
 159 const int prefered_acropolis_approach_angles[][3] = {
 160         {180, 180, 220},
 161         {130, 160, 200},
 162         {130, 160, 200},
 163         {130, 160, 200}, // FIXME... fix these
 164         {130, 160, 200}, // FIXME... fix these
 165         {130, 160, 200}, // FIXME... fix these
 166         {130, 160, 200}, // FIXME... fix these
 167         {130, 160, 200}, // FIXME... fix these
 168         {130, 160, 200}, // FIXME... fix these
 169         {130, 160, 200}, // 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 void robot_goto_acropolis(int phi)
 224 {
 225         robot_moveto_trans(
 226                 ACROPOLIS_CENTER_X + (ACROPOLIS_RADIUS + ROBOT_AXIS_TO_PUCK_M + 0.07)*cos(DEG2RAD(phi)),
 227                 ACROPOLIS_CENTER_Y + (ACROPOLIS_RADIUS + ROBOT_AXIS_TO_PUCK_M + 0.07)*sin(DEG2RAD(phi)),
 228                 ARRIVE_FROM(DEG2RAD(phi+180), 0.07), &tcSlow);
 229 }
 230
 231 void robot_goto_puck_in_grid(int nx, int ny, int phi)
 232 {
 233         struct puck_pos pp = free_puck_pos(nx, ny); // puck position
 234         robot_moveto_trans(
 235                 pp.x + ROBOT_AXIS_TO_PUCK_M*cos(DEG2RAD(phi)),
 236                 pp.y + ROBOT_AXIS_TO_PUCK_M*sin(DEG2RAD(phi)),
 237                 ARRIVE_FROM(DEG2RAD(phi+180), 0.1), &tcSlow);
 238 }
 239
 240 void robot_goto_next_puck_in_sequence(int lot, int puck_number)
 241 {
 242         robot_goto_puck_in_grid(
 243                 free_puck_pick_up_sequence[lot][puck_number][0],
 244                 free_puck_pick_up_sequence[lot][puck_number][1],
 245                 free_puck_pick_up_sequence[lot][puck_number][2]);
 246 }
 247
 248 /************************************************************************
 249  * FSM STATES DECLARATION
 250  ************************************************************************/
 251
 252 /* initial and starting states */
 253 FSM_STATE_DECL(init);
 254 FSM_STATE_DECL(wait_for_start);
 255 /* strategies related states */
 256 FSM_STATE_DECL(decide_what_next);
 257 FSM_STATE_DECL(collect_free_pucks);
 258 FSM_STATE_DECL(deposit_at_acropolis);
 259 /* movement states */
 260 FSM_STATE_DECL(approach_acropolis);
 261 FSM_STATE_DECL(simple_construction_zone_approach);
 262 FSM_STATE_DECL(approach_our_static_dispenser);
 263 FSM_STATE_DECL(approach_opponents_static_dispenser);
 264 /* States handling ACT's actions (SUBFSMs) */
 265 FSM_STATE_DECL(load_the_puck);
 266 FSM_STATE_DECL(grasp_the_puck);
 267 FSM_STATE_DECL(look_for_puck_ahead);
 268 FSM_STATE_DECL(look_around_for_puck);
 269
 270 /************************************************************************
 271  * INITIAL AND STARTING STATES
 272  ************************************************************************/
 273
 274 FSM_STATE(init)
 275 {
 276         switch (FSM_EVENT) {
 277         case EV_ENTRY:
 278                 act_camera_on();
 279                 free_puck_to_try_to_get_next = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
 280                 pucks_at_once = 4; // number of pucks to load at once (maximum number of pucks we want to have in robot)
 281                 tcFast = trajectoryConstraintsDefault;
 282                 //tcFast.maxv = 1.5;
 283                 tcSlow = trajectoryConstraintsDefault;
 284                 tcSlow.maxv = 0.2;
 285                 tcSlow.maxacc = 0.1;
 286                 tcSlow.maxomega = 1;
 287                 tcSlow.maxangacc = 1;
 288                 tcVerySlow = trajectoryConstraintsDefault;
 289                 tcVerySlow.maxv =  0.1;
 290                 tcVerySlow.maxacc = 0.05;
 291                 tcVerySlow.maxomega = 0.7;
 292                 tcVerySlow.maxangacc = 1;
 293                 FSM_TRANSITION(wait_for_start);
 294                 break;
 295         default:
 296                 break;
 297         }
 298 }
 299
 300 FSM_STATE(wait_for_start)
 301 {
 302         pthread_t thid;
 303 #ifdef COMPETITION
 304         printf("COMPETITION mode set\n");
 305 #endif
 306         switch (FSM_EVENT) {
 307 #ifdef WAIT_FOR_START
 308                 case EV_ENTRY:
 309                         break;
 310                 case EV_START:
 311 #else
 312                 case EV_ENTRY:
 313                 case EV_START:
 314 #endif
 315                         act_camera_off();
 316                         /* start competition timer */
 317                         pthread_create(&thid, NULL, timing_thread, NULL);
 318                         robot_set_est_pos_trans(0.16,
 319                                                 PLAYGROUND_HEIGHT_M - 0.16,
 320                                                 DEG2RAD(-45));
 321                         FSM_TRANSITION(decide_what_next);
 322                         break;
 323                 case EV_TIMER:
 324                 case EV_RETURN:
 325                 case EV_LASER_POWER:
 326                 case EV_GOAL_NOT_REACHABLE: // currently not used
 327                 case EV_SHORT_TIME_TO_END:
 328                 case EV_STACK_FULL:
 329                 case EV_ACTION_DONE:
 330                 case EV_ACTION_ERROR:
 331                 case EV_PUCK_REACHABLE:
 332                 case EV_MOTION_DONE:
 333                         DBG_PRINT_EVENT("unhandled event");
 334                         break;
 335                 case EV_EXIT:
 336                         break;
 337         }
 338 }
 339
 340 /************************************************************************
 341  * STRATEGIES RELATED STATES
 342  ************************************************************************/
 343
 344 FSM_STATE(decide_what_next)
 345 {
 346         switch (FSM_EVENT) {
 347                 case EV_ENTRY:
 348                         if(robot.pucks_inside == pucks_at_once) {
 349                                 FSM_TRANSITION(deposit_at_acropolis);
 350                         } else {
 351                                 if(robot.pucks_inside < pucks_at_once && free_puck_to_try_to_get_next < 6) {
 352                                         FSM_TRANSITION(collect_free_pucks);
 353                                 }
 354                         }
 355                         break;
 356                 case EV_MOTION_DONE:
 357                 case EV_ACTION_DONE:
 358                 case EV_RETURN:
 359                 case EV_TIMER:
 360                 case EV_LASER_POWER:
 361                 case EV_GOAL_NOT_REACHABLE: // currently not used
 362                 case EV_SHORT_TIME_TO_END:
 363                         FSM_TRANSITION(deposit_at_acropolis); //FIXME fsm transition to decide_what_next
 364                         break;                                  // everywhere and ...
 365                 case EV_STACK_FULL:
 366                 case EV_ACTION_ERROR:
 367                 case EV_PUCK_REACHABLE:
 368                 case EV_START:
 369                         DBG_PRINT_EVENT("unhandled event");
 370                         break;
 371                 case EV_EXIT:
 372                         break;
 373         }
 374 }
 375
 376 FSM_STATE(deposit_at_acropolis)
 377 {
 378         static int deposit_status;
 379         switch (FSM_EVENT) {
 380                 case EV_ENTRY:
 381                         deposit_status = 0;
 382                         SUBFSM_TRANSITION(approach_acropolis, NULL);
 383                         break;
 384                 case EV_RETURN: {
 385                         //if ((bool)FSM_EVENT_INT == true) {
 386                                 int floor = 2;
 387                                 FSM_SIGNAL(ACT, EV_PREPARE_THE_UNLOAD, (void*)floor);
 388                         //}
 389                         }
 390                         break;
 391                 case EV_ACTION_DONE:
 392                         switch(deposit_status) {
 393                         case 0:
 394                                 robot_move_by(0.11, NO_TURN(), &tcSlow);
 395                                 break;
 396                         case 1:
 397                                 robot_move_by(-0.07, NO_TURN(), &tcSlow);
 398                                 break;
 399                         }
 400                         deposit_status++;
 401                         break;
 402                 case EV_MOTION_DONE:
 403                         switch(deposit_status) {
 404                         case 1:
 405                                 FSM_SIGNAL(ACT, EV_UNLOAD_PUCKS, NULL);
 406                                 break;
 407                         case 2:
 408                                 robot_move_by(0.08, NO_TURN(), &tcVerySlow);
 409                                 deposit_status++;
 410                                 break;
 411                         case 3:
 412                                 robot_move_by(-0.15, NO_TURN(), &tcSlow);
 413                                 deposit_status++;
 414                                 break;
 415                         case 4:
 416                                 FSM_TRANSITION(decide_what_next);
 417                                 FSM_SIGNAL(ACT, EV_FREE_SPACE, NULL);
 418                                 deposit_status++;
 419                         }
 420                         break;
 421                 case EV_SHORT_TIME_TO_END:
 422                         //FSM_TRANSITION(deposit_at_acropolis);
 423                         break;
 424                 case EV_TIMER:
 425                 case EV_LASER_POWER:
 426                 case EV_GOAL_NOT_REACHABLE: // currently not used
 427                 case EV_STACK_FULL:
 428                 case EV_ACTION_ERROR:
 429                 case EV_PUCK_REACHABLE:
 430                 case EV_START:
 431                         DBG_PRINT_EVENT("unhandled event");
 432                         break;
 433                 case EV_EXIT:
 434                         break;
 435         }
 436 }
 437
 438 FSM_STATE(collect_free_pucks)
 439 {
 440         // game configuration... detected lot number is stored in robot.game_conf
 441         switch (FSM_EVENT) {
 442                 case EV_ENTRY:
 443                         robot_goto_next_puck_in_sequence(robot.game_conf, free_puck_to_try_to_get_next);
 444                         break;
 445                 case EV_MOTION_DONE: {
 446                                 printf("-----arrived where the free puck no. %d should be\n", free_puck_to_try_to_get_next);
 447                                 SUBFSM_TRANSITION(load_the_puck, NULL);
 448                         }
 449                         break;
 450                 case EV_RETURN:
 451                         switch(FSM_EVENT_RET_VAL) {
 452                         case LOAD_SUCCESS:
 453                                 printf(">>>>>> Loading the puck succeeded\n");
 454                                 break;
 455                         case LOAD_FAIL:
 456                                 printf(">>>>>> Loading the puck FAILED\n");
 457                                 break;
 458                         }
 459                         free_puck_to_try_to_get_next++;
 460                         FSM_TRANSITION(decide_what_next);
 461                         break;
 462                 case EV_SHORT_TIME_TO_END:
 463                         FSM_TRANSITION(deposit_at_acropolis);
 464                         break;
 465                 case EV_PUCK_REACHABLE:
 466                 case EV_ACTION_DONE:
 467                 case EV_TIMER:
 468                 case EV_LASER_POWER:
 469                 case EV_GOAL_NOT_REACHABLE: // currently not used
 470                 case EV_STACK_FULL:
 471                 case EV_ACTION_ERROR:
 472                 case EV_START:
 473                         DBG_PRINT_EVENT("unhandled event");
 474                         break;
 475                 case EV_EXIT:
 476                         break;
 477         }
 478 }
 479
 480 /************************************************************************
 481  * MOVEMENT STATES
 482  ************************************************************************/
 483
 484 FSM_STATE(approach_acropolis)
 485 {
 486         switch (FSM_EVENT) {
 487                 case EV_ENTRY:
 488                         robot_goto_acropolis(prefered_acropolis_approach_angles[(int)robot.game_conf][(free_puck_to_try_to_get_next-1)/2]);
 489                         break;
 490                 case EV_MOTION_DONE:
 491                         SUBFSM_RET(NULL);
 492                         // FIXME SUBFSM_RET((void*)true);
 493                         break;
 494                 case EV_GOAL_NOT_REACHABLE: // currently not used
 495                         break;
 496                 case EV_SHORT_TIME_TO_END:
 497                         break;
 498                 case EV_ACTION_DONE:
 499                 case EV_RETURN:
 500                 case EV_TIMER:
 501                 case EV_LASER_POWER:
 502                 case EV_STACK_FULL:
 503                 case EV_ACTION_ERROR:
 504                 case EV_PUCK_REACHABLE:
 505                 case EV_START:
 506                         DBG_PRINT_EVENT("unhandled event");
 507                         break;
 508                 case EV_EXIT:
 509                         break;
 510         }
 511 }
 512
 513 FSM_STATE(simple_construction_zone_approach)
 514 {
 515         switch (FSM_EVENT) {
 516                 case EV_ENTRY:
 517                         robot_trajectory_new(&tcFast);
 518                         robot_trajectory_add_point_trans(0.9, 1);
 519                         robot_trajectory_add_final_point_trans(0.9, ROBOT_AXIS_TO_FRONT_M + 0.05, NO_TURN());
 520                         break;
 521                 case EV_MOTION_DONE:
 522                         //FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
 523                         break;
 524                 case EV_SHORT_TIME_TO_END:
 525                         FSM_TRANSITION(deposit_at_acropolis);
 526                         break;
 527                 case EV_ACTION_DONE:
 528                 case EV_RETURN:
 529                 case EV_TIMER:
 530                 case EV_LASER_POWER:
 531                 case EV_GOAL_NOT_REACHABLE: // currently not used
 532                 case EV_STACK_FULL:
 533                 case EV_ACTION_ERROR:
 534                 case EV_PUCK_REACHABLE:
 535                 case EV_START:
 536                         DBG_PRINT_EVENT("unhandled event");
 537                         break;
 538                 case EV_EXIT:
 539                         break;
 540         }
 541 }
 542
 543 FSM_STATE(approach_our_static_dispenser)
 544 {
 545         switch (FSM_EVENT) {
 546                 case EV_ENTRY:  {
 547                         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 548                         tc.maxv /= 1.0;
 549                         robot_trajectory_new(&tc);
 550
 551                         robot_trajectory_add_point_trans(STATIC_DISPENSER_X,
 552                                                          STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
 553                         robot_trajectory_add_final_point_trans(STATIC_DISPENSER_X,
 554                                                                STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
 555                                                                NO_TURN());
 556                         }
 557                         break;
 558                 case EV_MOTION_DONE:
 559                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 560                         printf("Arrived to the static dispenser\n");
 561                         break;
 562                 case EV_ACTION_DONE:
 563                         printf("A Puck picked up\n");
 564                         break;
 565                 case EV_SHORT_TIME_TO_END:
 566                         FSM_TRANSITION(deposit_at_acropolis);
 567                         break;
 568                 case EV_RETURN:
 569                 case EV_TIMER:
 570                 case EV_LASER_POWER:
 571                 case EV_GOAL_NOT_REACHABLE: // currently not used
 572                 case EV_STACK_FULL:
 573                 case EV_ACTION_ERROR:
 574                 case EV_PUCK_REACHABLE:
 575                 case EV_START:
 576                         DBG_PRINT_EVENT("unhandled event");
 577                         break;
 578                 case EV_EXIT:
 579                         break;
 580         }
 581 }
 582
 583 FSM_STATE(approach_opponents_static_dispenser)
 584 {
 585         switch (FSM_EVENT) {
 586                 case EV_ENTRY:  {
 587                         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 588                         tc.maxv /= 1.0;
 589                         robot_trajectory_new(&tc);
 590
 591                         robot_trajectory_add_point_trans(OPPONENTS_STATIC_DISPENSER_X,
 592                                                          OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
 593                         robot_trajectory_add_final_point_trans(OPPONENTS_STATIC_DISPENSER_X,
 594                                                                OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
 595                                                                NO_TURN());
 596                         }
 597                         break;
 598                 case EV_MOTION_DONE:
 599                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 600                         printf("Arrived to the static dispenser\n");
 601                         break;
 602                 case EV_ACTION_DONE:
 603                         printf("A Puck picked up\n");
 604                         break;
 605                 case EV_SHORT_TIME_TO_END:
 606                         FSM_TRANSITION(deposit_at_acropolis);
 607                         break;
 608                 case EV_RETURN:
 609                 case EV_TIMER:
 610                 case EV_LASER_POWER:
 611                 case EV_GOAL_NOT_REACHABLE: // currently not used
 612                 case EV_STACK_FULL:
 613                 case EV_ACTION_ERROR:
 614                 case EV_PUCK_REACHABLE:
 615                 case EV_START:
 616                         DBG_PRINT_EVENT("unhandled event");
 617                         break;
 618                 case EV_EXIT:
 619                         break;
 620         }
 621 }
 622
 623 /************************************************************************
 624  * STATES HANDLING ACT's ACTIONS (to be used as SUB FSMs)
 625  ************************************************************************/
 626
 627 FSM_STATE(load_the_puck)
 628 {
 629         static int puck_load_attempt_count;
 630         switch (FSM_EVENT) {
 631                 case EV_ENTRY:
 632                         puck_load_attempt_count = 0;
 633                         FSM_SIGNAL(ACT, EV_SCRABBLE, NULL);
 634                         FSM_TIMER(200);
 635                         break;
 636                 case EV_TIMER:
 637                         robot_move_by(0.02, NO_TURN(), &tcSlow);
 638                         break;
 639                 case EV_MOTION_DONE:
 640                         FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
 641                         break;
 642                 case EV_ACTION_DONE:
 643                         SUBFSM_RET((void *)LOAD_SUCCESS);
 644                         break;
 645                 case EV_ACTION_ERROR:
 646                         puck_load_attempt_count++;
 647                         if (puck_load_attempt_count > 2) {
 648                                 SUBFSM_RET((void *)LOAD_FAIL);
 649                         } else {
 650                                 robot_move_by(0.02, NO_TURN(), &tcSlow);
 651                         }
 652                         break;
 653                 case EV_SHORT_TIME_TO_END:
 654                         FSM_TRANSITION(deposit_at_acropolis);
 655                         break;
 656                 case EV_RETURN:
 657                 case EV_LASER_POWER:
 658                 case EV_GOAL_NOT_REACHABLE: // currently not used
 659                 case EV_STACK_FULL:
 660                 case EV_PUCK_REACHABLE:
 661                 case EV_START:
 662                         DBG_PRINT_EVENT("unhandled event");
 663                         break;
 664                 case EV_EXIT:
 665                         break;
 666         }
 667 }
 668
 669 /* of no use without the sharp sensor measuring "puck distance"
 670 FSM_STATE(look_around_for_puck)
 671 {
 672         static int lfp_status = 0;
 673         const static int scatter_angle = 20;
 674         static struct robot_pos_type orig_position;
 675         switch (FSM_EVENT) {
 676                 case EV_ENTRY:
 677                         ROBOT_LOCK(ref_pos);
 678                         orig_position = robot.ref_pos;
 679                         ROBOT_UNLOCK(ref_pos);
 680                         //printf("original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 681                         robot_move_by(0, TURN_CW(DEG2RAD(RAD2DEG(orig_position.phi)-scatter_angle)), &tcSlow);
 682                         //printf("lfp_status: %d\n", lfp_status);
 683                         break;
 684                 case EV_MOTION_DONE:
 685                         switch (lfp_status) {
 686                         case 0:
 687                                 //printf("2. original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 688                                 //printf("robot.fer_pos angle of rotation of the robot: %f degrees\n", RAD2DEG(robot.ref_pos.phi));
 689                                 //printf("--- robot move by ... turn cw\n");
 690                                 robot_move_by(0, TURN_CCW(DEG2RAD(RAD2DEG(orig_position.phi)+scatter_angle)), &tcSlow);
 691                                 lfp_status++;
 692                                 break;
 693                         case 1:
 694                                 robot_move_by(0, TURN(orig_position.phi), &tcSlow);
 695                                 lfp_status++;
 696                                 break;
 697                         case 2: // puck not found
 698                                 SUBFSM_RET((void *)LOOK_AROUND_FAIL);
 699                                 break;
 700                         }
 701                         //printf("lfp_status: %d\n", lfp_status);
 702                         break;
 703                 case EV_PUCK_REACHABLE: // puck found
 704                         robot_stop();
 705                         SUBFSM_RET((void *)LOOK_AROUND_SUCCESS);
 706                         break;
 707                 case EV_ACTION_DONE:
 708                 case EV_ACTION_ERROR: // look for puck does not send this event
 709                 case EV_RETURN:
 710                 case EV_TIMER:
 711                 case EV_LASER_POWER:
 712                 case EV_GOAL_NOT_REACHABLE: // currently not used
 713                 case EV_SHORT_TIME_TO_END:
 714                 case EV_STACK_FULL:
 715                 case EV_START:
 716                         DBG_PRINT_EVENT("unhandled event");
 717                         break;
 718                 case EV_EXIT:
 719                         break;
 720         }
 721 }
 722 */
 723
 724
 725 /*
 726 FSM_STATE()
 727 {
 728         switch (FSM_EVENT) {
 729                 case EV_ENTRY:
 730                         break;
 731                 case EV_SHORT_TIME_TO_END:
 732                 case EV_MOTION_DONE:
 733                 case EV_ACTION_DONE:
 734                 case EV_RETURN:
 735                 case EV_TIMER:
 736                 case EV_LASER_POWER:
 737                 case EV_GOAL_NOT_REACHABLE: // currently not used
 738                 case EV_STACK_FULL:
 739                 case EV_ACTION_ERROR:
 740                 case EV_PUCK_REACHABLE:
 741                 case EV_START:
 742                         DBG_PRINT_EVENT("unhandled event");
 743                         break;
 744                 case EV_EXIT:
 745                         break;
 746         }
 747 }
 748 */
 749
 750
 751 int main()
 752 {
 753         int rv;
 754
 755         rv = robot_init();
 756         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 757
 758         robot.fsm.main.debug_states = 1;
 759         robot.fsm.motion.debug_states = 1;
 760         robot.fsm.act.debug_states = 1;
 761
 762         robot.fsm.main.state = &fsm_state_main_init;
 763
 764         rv = robot_start();
 765         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 766
 767         robot_destroy();
 768
 769         return 0;
 770 }