src/robofsm/common-states.cc

   1 #define FSM_MAIN
   2 #include "robodata.h"
   3 #include <robot.h>
   4 #include <fsm.h>
   5 #include <unistd.h>
   6 #include <math.h>
   7 #include <movehelper.h>
   8 #include <map.h>
   9 #include <sharp.h>
  10 #include <robomath.h>
  11 #include <string.h>
  12 #include <robodim.h>
  13 #include <error.h>
  14 #include "actuators.h"
  15 #include <sharp.h>
  16 #include <trgen.h>
  17 #include "match-timing.h"
  18 #include <stdbool.h>
  19 #include <ul_log.h>
  20
  21 UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
  22
  23 #include "common-states.h"
  24
  25 /************************************************************************
  26  * Functions used in and called from all the (almost identical)
  27  * "wait for start" states in particular strategies.
  28  ************************************************************************/
  29
  30 #undef DBG_FSM_STATE
  31 #define DBG_FSM_STATE(name)     do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
  32                                                                    fsm->debug_name, robot_current_time(), \
  33                                                                    name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
  34
  35
  36 void set_initial_position()
  37 {
  38         robot_set_est_pos_trans(ROBOT_START_X_M,
  39                                 ROBOT_START_Y_M,
  40                                 DEG2RAD(ROBOT_START_ANGLE_DEG));
  41 }
  42
  43 void actuators_home()
  44 {
  45         act_jaws(CLOSE);
  46
  47         bool pokus = robot.fsm.motion.state_name == "wait_and_try_again";
  48         // drive lift to home position
  49         //act_lift(0, 0, 1);
  50         // unset the homing request
  51         //act_lift(0, 0, 0);
  52 }
  53
  54 void start_entry()
  55 {
  56         pthread_t thid;
  57         robot.check_turn_safety = false;
  58         pthread_create(&thid, NULL, timing_thread, NULL);
  59         start_timer();
  60 }
  61
  62 // We set initial position periodically in order for it to be updated
  63 // on the display if the team color is changed during waiting for
  64 // start.
  65 void start_timer()
  66 {
  67         set_initial_position();
  68         if (robot.start_state == START_PLUGGED_IN)
  69                 actuators_home();
  70 }
  71
  72 void start_go()
  73 {
  74         sem_post(&robot.start);
  75         actuators_home();
  76         set_initial_position();
  77 }
  78
  79 void start_exit()
  80 {
  81
  82 }
  83
  84 bool read_sharp()
  85 {
  86         int sharp_data = robot.orte.jaws_status.act_pos.left;
  87         int sharp_dist = 0;
  88         sharp_dist = s_gp2y0a21_ir2mmLong(sharp_data);
  89         printf("sharp data: %dmm\n", sharp_dist);
  90         return (sharp_dist <= 150 ? true : false);
  91 }
  92
  93 void inline enable_bumpers(bool enabled)
  94 {
  95         robot.use_left_bumper = enabled;
  96         robot.use_right_bumper = enabled;
  97         robot.use_back_bumpers = enabled;
  98 }
  99
 100 void enable_my_square_walls(bool enabled)
 101 {
 102         for (int i = 0; i < 15; i++) {
 103                 if (robot.team_color == RED)
 104                         ShmapSetCircleFlag(red_sq[i].x, red_sq[i].y, 0.2, enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
 105                 else
 106                         ShmapSetCircleFlag(blue_sq[i].x, blue_sq[i].y, 0.2,enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
 107         }
 108 }
 109
 110 /************************************************************************
 111  * Trajectory constraints used; They are  initialized in the main() function in competition.cc
 112  ************************************************************************/
 113
 114 struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
 115
 116 bool tower = false;
 117 bool bonus_placed = false;
 118
 119 const double load_dist = 0.04; // TODO distance from side when loading green figures
 120 const double app_dist = 0.04;  // TODO distance from green figures when approach
 121
 122 /* fields with numbers of opponent squares, we have five variations of movement style across opp. squares*/
 123 const int move_around[][8] = {{12, 14, 11, 13, 11, 8, 6, 9},
 124                             {6, 8, 11, 13, 11, 14, 12, 9},
 125                             {11, 13, 11, 8, 11, 14, 12, 9},
 126                             {6, 8, 11, 13, 11, 14, 12, 9},
 127                             {11, 8, 11, 13, 11, 14, 12, 9}};
 128
 129 /** generate "random" positions on oponent squares and goto this position */
 130 FSM_STATE(move_around)
 131 {
 132         static int next_sq = 0;
 133         static int max_wait = 0;
 134         // choose randomly one of five move_around strategies
 135         static int strategy = rand() % 5;
 136         int goal;
 137         double goal_x, goal_y;
 138         static bool entry = true;
 139
 140         switch(FSM_EVENT) {
 141                 case EV_ENTRY:
 142                         max_wait = 0;
 143                         FSM_TIMER(2000);
 144                         robot.use_left_bumper = true;
 145                         robot.use_right_bumper = true;
 146                         robot.use_back_bumpers = true;
 147                         robot.ignore_hokuyo = false;
 148
 149                         act_jaws(CLOSE);
 150
 151                         enable_my_square_walls(true);
 152
 153                         srand((int)(robot_current_time()*10000));
 154
 155                         // save next square number where to go to
 156                         goal = move_around[strategy][next_sq];
 157
 158                         // pick position on opponent squares with goal index
 159                         if (robot.team_color == RED) {
 160                                 goal_x = blue_sq[goal].x;
 161                                 goal_y = blue_sq[goal].y;
 162                         } else {
 163                                 goal_x = red_sq[goal].x;
 164                                 goal_y = red_sq[goal].y;
 165                         }
 166
 167                         robot_goto_notrans(goal_x, goal_y, NO_TURN(), &tcFast);
 168                         printf("strategy: %d, next_sq: %d, goal: %d\n",strategy, next_sq, goal);
 169                         next_sq = (next_sq + 1 < sizeof(move_around[strategy]) ? next_sq + 1 : next_sq);
 170                         break;
 171                 case EV_START:
 172                 case EV_TIMER:
 173                         // every two seconds check if short time to end
 174                         FSM_TIMER(2000);
 175                         if (robot.short_time_to_end == true) {
 176                                 // if short time to end - send stop signal to motion FSM and return to main FSM
 177                                 FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
 178                                 SUBFSM_RET(NULL);
 179                                 printf("return from move around state\n");
 180                         } else if (robot.fsm.motion.state_name == "wait_and_try_again") {
 181                                 // if goal position unawailable (obstacle) try max. X-times
 182                                 if (++max_wait >= 3) {
 183                                         // if goal not awailable, send stop signal to motion FSM and generate new target trom move_around
 184                                         printf("go to next square!\n");
 185                                         max_wait = 0;
 186                                         next_sq = (next_sq + 1 < sizeof(move_around[strategy]) ? next_sq + 1 : next_sq);
 187                                         FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
 188                                         FSM_TRANSITION(move_around);
 189                                  } else {
 190                                          printf("waiting for opponent! %d\n", max_wait);
 191                                  }
 192                         }
 193                         break;
 194                 case EV_RETURN:
 195                 case EV_MOTION_DONE:
 196                         if (robot.short_time_to_end == true)
 197                                 SUBFSM_RET(NULL);
 198                         else
 199                                 FSM_TRANSITION(move_around);
 200                         break;
 201                 case EV_MOTION_ERROR:
 202                 case EV_EXIT:
 203                         break;
 204         }
 205 }
 206
 207 /** pick figure from opponent bonus square */
 208 FSM_STATE(approach_opp_bonus_figure)
 209 {
 210         switch(FSM_EVENT) {
 211                 case EV_ENTRY:
 212                         robot.use_left_bumper = true;
 213                         robot.use_right_bumper = true;
 214                         robot.use_back_bumpers = true;
 215                         robot.ignore_hokuyo = false;
 216
 217                         robot_goto_trans(
 218                                 0.45 + 2*0.35 + 0.175,
 219                                 0.35 + 0.175,
 220                                 TURN(DEG2RAD(-50)), &tcFast);
 221                         break;
 222                 case EV_MOTION_DONE:
 223                         act_jaws(OPEN);
 224                         FSM_TRANSITION(load_opp_bonus_figure);
 225                         break;
 226                 case EV_START:
 227                 case EV_TIMER:
 228                 case EV_RETURN:
 229                 case EV_MOTION_ERROR:
 230                 case EV_SWITCH_STRATEGY:
 231                         DBG_PRINT_EVENT("unhandled event");
 232                 case EV_EXIT:
 233                         break;
 234         }
 235 }
 236
 237 FSM_STATE(load_opp_bonus_figure)
 238 {
 239         switch(FSM_EVENT) {
 240                 case EV_ENTRY:
 241                         enable_my_square_walls(false);
 242                         robot.use_left_bumper = true;
 243                         robot.use_right_bumper = true;
 244                         robot.use_back_bumpers = true;
 245                         robot.ignore_hokuyo = true;
 246
 247                         //robot_trajectory_new(&tcSlow);
 248                         robot_move_by(0.35, NO_TURN(), &tcSlow);
 249                         break;
 250                 case EV_MOTION_DONE:
 251                         act_jaws(CLOSE);
 252                         FSM_TRANSITION(place_opp_bonus_figure);
 253                         break;
 254                 case EV_START:
 255                 case EV_TIMER:
 256                 case EV_RETURN:
 257                 case EV_MOTION_ERROR:
 258                 case EV_SWITCH_STRATEGY:
 259                         DBG_PRINT_EVENT("unhandled event");
 260                 case EV_EXIT:
 261                         break;
 262         }
 263 }
 264
 265 FSM_STATE(place_opp_bonus_figure)
 266 {
 267         switch(FSM_EVENT) {
 268                 case EV_ENTRY:
 269                         robot.use_left_bumper = true;
 270                         robot.use_right_bumper = true;
 271                         robot.use_back_bumpers = true;
 272                         robot.ignore_hokuyo = true;
 273
 274                         robot_trajectory_new_backward(&tcSlow);
 275                         robot_trajectory_add_point_trans(
 276                                 0.45 + 2*0.35 + 0.175,
 277                                 0.35 + 0.2);
 278                         robot_trajectory_add_final_point_trans(
 279                                 0.45 + 2*0.35 + 0.175,
 280                                 2*0.35 + 0.175 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
 281                                 ARRIVE_FROM(DEG2RAD(90), 0.1));
 282                         break;
 283                 case EV_MOTION_DONE:
 284                         act_jaws(OPEN);
 285                         FSM_TRANSITION(leave_opp_bonus_figure);
 286                         break;
 287                 case EV_START:
 288                 case EV_TIMER:
 289                 case EV_RETURN:
 290                 case EV_MOTION_ERROR:
 291                 case EV_SWITCH_STRATEGY:
 292                         DBG_PRINT_EVENT("unhandled event");
 293                 case EV_EXIT:
 294                         break;
 295         }
 296 }
 297
 298 FSM_STATE(leave_opp_bonus_figure)
 299 {
 300         switch(FSM_EVENT) {
 301                 case EV_ENTRY:
 302                         robot.use_left_bumper = true;
 303                         robot.use_right_bumper = true;
 304                         robot.use_back_bumpers = true;
 305                         robot.ignore_hokuyo = true;
 306
 307                         robot_trajectory_new_backward(&tcSlow);
 308                         robot_trajectory_add_final_point_trans(
 309                                 0.45 + 2*0.35 + 0.175,
 310                                 5*0.35 + 0.1,
 311                                 NO_TURN());
 312                         break;
 313                 case EV_MOTION_DONE:
 314                         act_jaws(CLOSE);
 315                         SUBFSM_RET();
 316                         break;
 317                 case EV_START:
 318                 case EV_TIMER:
 319                 case EV_RETURN:
 320                 case EV_MOTION_ERROR:
 321                 case EV_SWITCH_STRATEGY:
 322                         DBG_PRINT_EVENT("unhandled event");
 323                 case EV_EXIT:
 324                         break;
 325         }
 326 }
 327
 328 /** securely bypass firt figure in front of starting area */
 329 FSM_STATE(bypass_figure_in_front_of_start)
 330 {
 331         switch(FSM_EVENT) {
 332                 case EV_ENTRY:
 333                         robot.use_left_bumper = true;
 334                         robot.use_right_bumper = true;
 335                         robot.use_back_bumpers = false;
 336                         robot.ignore_hokuyo = false;
 337
 338                         robot_trajectory_new(&tcFast);
 339                         robot_trajectory_add_point_trans(
 340                                 0.45 + 0.3,
 341                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
 342                         robot_trajectory_add_final_point_trans(
 343                                 0.45 + 0.3,
 344                                 4*0.35,
 345                                 NO_TURN());
 346                         break;
 347                 case EV_MOTION_DONE:
 348                         SUBFSM_RET(NULL);
 349                         break;
 350                 case EV_START:
 351                 case EV_TIMER:
 352                 case EV_RETURN:
 353                 case EV_MOTION_ERROR:
 354                 case EV_SWITCH_STRATEGY:
 355                         DBG_PRINT_EVENT("unhandled event");
 356                 case EV_EXIT:
 357                         break;
 358         }
 359 }
 360
 361 /** pick second figure from green area */
 362 FSM_STATE(approach_second_green_figure)
 363 {
 364         switch(FSM_EVENT) {
 365                 case EV_ENTRY:
 366                         robot.use_left_bumper = true;
 367                         robot.use_right_bumper = true;
 368                         robot.use_back_bumpers = true;
 369                         robot.ignore_hokuyo = false;
 370
 371                         robot_trajectory_new(&tcFast);
 372                         robot_trajectory_add_final_point_trans(
 373                                 0.45 + 0.3 - app_dist,
 374                                 0.29 + 0.28,
 375                                 TURN(DEG2RAD(180)));
 376                         break;
 377                 case EV_MOTION_DONE:
 378                         act_jaws(OPEN);
 379                         FSM_TIMER(1000);
 380                         break;
 381                 case EV_TIMER:
 382                         FSM_TRANSITION(load_second_green_figure);
 383                         break;
 384                 case EV_RETURN:
 385                 case EV_MOTION_ERROR:
 386                 case EV_EXIT:
 387                         break;
 388         }
 389 }
 390
 391 FSM_STATE(load_second_green_figure)
 392 {
 393         switch(FSM_EVENT) {
 394                 case EV_ENTRY:
 395                         robot.use_left_bumper = true;
 396                         robot.use_right_bumper = true;
 397                         robot.use_back_bumpers = true;
 398                         robot.ignore_hokuyo = true;
 399
 400                         robot_trajectory_new(&tcSlow);
 401                         robot_trajectory_add_final_point_trans(
 402                                 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + load_dist,
 403                                 0.29 + 0.28,
 404                                 ARRIVE_FROM(DEG2RAD(180), 0.10));
 405                         break;
 406                 case EV_MOTION_DONE:
 407                         FSM_TIMER(1000);
 408                         act_jaws(CLOSE);
 409                         break;
 410                 case EV_TIMER:
 411                         FSM_TRANSITION(go_out_second_green_figure);
 412                         break;
 413                 case EV_RETURN:
 414                 case EV_MOTION_ERROR:
 415                 case EV_EXIT:
 416                         break;
 417         }
 418 }
 419
 420 FSM_STATE(go_out_second_green_figure)
 421 {
 422         switch(FSM_EVENT) {
 423                 case EV_ENTRY:
 424                         robot.use_left_bumper = true;
 425                         robot.use_right_bumper = true;
 426                         robot.use_back_bumpers = true;
 427                         robot.ignore_hokuyo = true;
 428
 429                         robot_trajectory_new_backward(&tcFast);
 430                         robot_trajectory_add_final_point_trans(
 431                                 0.45 + 0.3,
 432                                 0.7,
 433                                 NO_TURN());
 434                         break;
 435                 case EV_MOTION_DONE:
 436                         if (read_sharp()) {
 437                                 FSM_TRANSITION(place_figure_to_bonus_area);
 438                         } else {
 439                                 FSM_TRANSITION(place_figure_to_protected_block);
 440                         }
 441                         break;
 442                 case EV_TIMER:
 443                         break;
 444                 case EV_START:
 445                 case EV_RETURN:
 446                 case EV_MOTION_ERROR:
 447                 case EV_EXIT:
 448                         break;
 449         }
 450 }
 451
 452 FSM_STATE(place_figure_to_protected_block)
 453 {
 454         switch(FSM_EVENT) {
 455                 case EV_ENTRY:
 456                         robot.use_left_bumper = true;
 457                         robot.use_right_bumper = true;
 458                         robot.use_back_bumpers = true;
 459                         robot.ignore_hokuyo = false;
 460
 461                         robot_trajectory_new(&tcSlow);
 462                         robot_trajectory_add_final_point_trans(
 463                                 0.45 + 0.175,
 464                                 0.12 + 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
 465                                 ARRIVE_FROM(DEG2RAD(-90), 0.20));
 466                         break;
 467                 case EV_START:
 468                 case EV_TIMER:
 469                         FSM_TRANSITION(leave_protected_figure);
 470                         break;
 471                 case EV_RETURN:
 472                 case EV_MOTION_DONE:
 473                         act_jaws(OPEN);
 474                         FSM_TIMER(1000);
 475                         break;
 476                 case EV_MOTION_ERROR:
 477                 case EV_SWITCH_STRATEGY:
 478                         DBG_PRINT_EVENT("unhandled event");
 479                 case EV_EXIT:
 480                         break;
 481         }
 482 }
 483
 484 FSM_STATE(leave_protected_figure)
 485 {
 486         switch(FSM_EVENT) {
 487                 case EV_ENTRY:
 488                         robot.use_left_bumper = true;
 489                         robot.use_right_bumper = true;
 490                         robot.use_back_bumpers = true;
 491                         robot.ignore_hokuyo = false;
 492
 493                         robot_trajectory_new_backward(&tcFast);
 494                         robot_trajectory_add_point_trans(
 495                                 0.45 + 0.175,
 496                                 0.7);
 497                         robot_trajectory_add_final_point_trans(
 498                                 0.45 + 0.3,
 499                                 0.29 + 2*0.28,
 500                                 NO_TURN());
 501                         break;
 502                 case EV_START:
 503                 case EV_TIMER:
 504                         break;
 505                 case EV_RETURN:
 506                 case EV_MOTION_DONE:
 507                         act_jaws(CLOSE);
 508                         SUBFSM_RET(NULL);
 509                         break;
 510                 case EV_MOTION_ERROR:
 511                 case EV_EXIT:
 512                         break;
 513         }
 514 }
 515
 516 /** pick third figure from green area */
 517 FSM_STATE(approach_third_green_figure)
 518 {
 519         switch(FSM_EVENT) {
 520                 case EV_ENTRY:
 521                         robot.use_left_bumper = true;
 522                         robot.use_right_bumper = true;
 523                         robot.use_back_bumpers = true;
 524                         robot.ignore_hokuyo = false;
 525
 526                         robot_trajectory_new(&tcFast);
 527                         robot_trajectory_add_final_point_trans(
 528                                 0.45 + 0.3 - app_dist,
 529                                 0.29 + 2*0.28,
 530                                 TURN(DEG2RAD(180)));
 531                         break;
 532                 case EV_MOTION_DONE:
 533                         act_jaws(OPEN);
 534                         FSM_TIMER(1000);
 535                         break;
 536                 case EV_TIMER:
 537                         FSM_TRANSITION(load_third_green_figure);
 538                         break;
 539                 case EV_RETURN:
 540                 case EV_MOTION_ERROR:
 541                 case EV_EXIT:
 542                         break;
 543         }
 544 }
 545
 546 FSM_STATE(load_third_green_figure)
 547 {
 548         switch(FSM_EVENT) {
 549                 case EV_ENTRY:
 550                         robot.use_left_bumper = true;
 551                         robot.use_right_bumper = true;
 552                         robot.use_back_bumpers = true;
 553                         robot.ignore_hokuyo = true;
 554
 555                         robot_trajectory_new(&tcSlow);
 556                         robot_trajectory_add_final_point_trans(
 557                                 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + load_dist,
 558                                 0.29 + 2*0.28,
 559                                 ARRIVE_FROM(DEG2RAD(180), 0.20));
 560                         break;
 561                 case EV_MOTION_DONE:
 562                         act_jaws(CLOSE);
 563                         FSM_TIMER(1000);
 564                         break;
 565                 case EV_TIMER:
 566                         FSM_TRANSITION(go_out_third_green_figure);
 567                         break;
 568                 case EV_RETURN:
 569                 case EV_MOTION_ERROR:
 570                 case EV_EXIT:
 571                         break;
 572         }
 573 }
 574
 575 FSM_STATE(go_out_third_green_figure)
 576 {
 577         switch(FSM_EVENT) {
 578                 case EV_ENTRY:
 579                         robot.use_left_bumper = true;
 580                         robot.use_right_bumper = true;
 581                         robot.use_back_bumpers = true;
 582                         robot.ignore_hokuyo = true;
 583
 584                         robot_trajectory_new_backward(&tcFast);
 585                         robot_trajectory_add_final_point_trans(
 586                                 0.45 + 0.35,
 587                                 0.29 + 2*0.28,
 588                                 NO_TURN());
 589                         break;
 590                 case EV_MOTION_DONE:
 591                         if (bonus_placed)
 592                                 FSM_TRANSITION(place_figure_to_near_area);
 593                         else
 594                                 FSM_TRANSITION(place_figure_to_bonus_area);
 595                         break;
 596                 case EV_TIMER:
 597                         break;
 598                 case EV_START:
 599                 case EV_RETURN:
 600                 case EV_MOTION_ERROR:
 601                 case EV_EXIT:
 602                         robot.ignore_hokuyo = false;
 603                         break;
 604         }
 605 }
 606
 607 FSM_STATE(place_figure_to_near_area)
 608 {
 609         switch(FSM_EVENT) {
 610                 case EV_ENTRY:
 611                         robot.use_left_bumper = true;
 612                         robot.use_right_bumper = true;
 613                         robot.use_back_bumpers = true;
 614                         robot.ignore_hokuyo = false;
 615
 616                         robot_trajectory_new(&tcFast);
 617                         robot_trajectory_add_final_point_trans(
 618                                 0.45 + 0.35 + 0.175,
 619                                 0.35 + 0.175 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
 620                                 TURN(DEG2RAD(-90)));
 621                         break;
 622                 case EV_MOTION_DONE:
 623                         act_jaws(OPEN);
 624                         FSM_TIMER(1000);
 625                         break;
 626                 case EV_TIMER:
 627                         FSM_TRANSITION(leave_near_figure);
 628                         break;
 629                 case EV_START:
 630                 case EV_RETURN:
 631                 case EV_MOTION_ERROR:
 632                 case EV_EXIT:
 633                         break;
 634         }
 635 }
 636
 637 FSM_STATE(leave_near_figure)
 638 {
 639         switch(FSM_EVENT) {
 640                 case EV_ENTRY:
 641                         robot.use_left_bumper = true;
 642                         robot.use_right_bumper = true;
 643                         robot.use_back_bumpers = true;
 644                         robot.ignore_hokuyo = false;
 645
 646                         robot_trajectory_new_backward(&tcFast);
 647                         robot_trajectory_add_final_point_trans(
 648                                 0.45 + 0.35 + 0.175,
 649                                 0.29 + 3*0.28,
 650                                 NO_TURN());
 651                         break;
 652                 case EV_MOTION_DONE:
 653                         act_jaws(CLOSE);
 654                         FSM_TIMER(1000);
 655                         break;
 656                 case EV_TIMER:
 657                         SUBFSM_RET(NULL);
 658                         break;
 659                 case EV_START:
 660                 case EV_RETURN:
 661                 case EV_MOTION_ERROR:
 662                 case EV_EXIT:
 663                         break;
 664         }
 665 }
 666
 667 FSM_STATE(place_figure_to_bonus_area)
 668 {
 669         switch(FSM_EVENT) {
 670                 case EV_ENTRY:
 671                         robot.use_left_bumper = true;
 672                         robot.use_right_bumper = true;
 673                         robot.use_back_bumpers = true;
 674                         robot.ignore_hokuyo = false;
 675
 676                         robot_trajectory_new(&tcSlow);
 677                         robot_trajectory_add_final_point_trans(
 678                                 0.45 + 0.7 + 0.175,
 679                                 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
 680                                 ARRIVE_FROM(DEG2RAD(-90), 0.3));
 681                         break;
 682                 case EV_START:
 683                 case EV_TIMER:
 684                         FSM_TRANSITION(leave_bonus_figure);
 685                         break;
 686                 case EV_RETURN:
 687                 case EV_MOTION_DONE:
 688                         bonus_placed = true;
 689                         act_jaws(OPEN);
 690                         FSM_TIMER(1000);
 691                         break;
 692                 case EV_MOTION_ERROR:
 693                 case EV_SWITCH_STRATEGY:
 694                         DBG_PRINT_EVENT("unhandled event");
 695                 case EV_EXIT:
 696                         break;
 697         }
 698 }
 699
 700 FSM_STATE(leave_bonus_figure)
 701 {
 702         switch(FSM_EVENT) {
 703                 case EV_ENTRY:
 704                         robot.use_left_bumper = true;
 705                         robot.use_right_bumper = true;
 706                         robot.use_back_bumpers = true;
 707                         robot.ignore_hokuyo = true;
 708
 709                         robot_trajectory_new_backward(&tcFast);
 710                         robot_trajectory_add_final_point_trans(
 711                                 0.45 + 0.7 + 0.2,
 712                                 0.7,
 713                                 NO_TURN());
 714                         break;
 715                 case EV_START:
 716                 case EV_TIMER:
 717                         break;
 718                 case EV_RETURN:
 719                 case EV_MOTION_DONE:
 720                         act_jaws(CLOSE);
 721                         act_lift(DOWN, 0, 0);
 722                         SUBFSM_RET(NULL);
 723                         break;
 724                 case EV_MOTION_ERROR:
 725                 case EV_EXIT:
 726                         break;
 727         }
 728 }
 729
 730 /** pick fourth green figure from green area */
 731 FSM_STATE(approach_fourth_green_figure)
 732 {
 733         switch(FSM_EVENT) {
 734                 case EV_ENTRY:
 735                         robot.use_left_bumper = true;
 736                         robot.use_right_bumper = true;
 737                         robot.use_back_bumpers = true;
 738                         robot.ignore_hokuyo = false;
 739
 740                         robot_trajectory_new(&tcFast);
 741                         robot_trajectory_add_final_point_trans(
 742                                 0.45 + 0.3 - app_dist,
 743                                 0.29 + 3*0.28,
 744                                 TURN(DEG2RAD(180)));
 745                         break;
 746                 case EV_MOTION_DONE:
 747                         tower = read_sharp();
 748                         act_jaws(OPEN);
 749                         FSM_TIMER(1000);
 750                         break;
 751                 case EV_TIMER:
 752                         FSM_TRANSITION(load_fourth_green_figure);
 753                         break;
 754                 case EV_RETURN:
 755                 case EV_MOTION_ERROR:
 756                 case EV_EXIT:
 757                         break;
 758         }
 759 }
 760
 761 FSM_STATE(load_fourth_green_figure)
 762 {
 763         switch(FSM_EVENT) {
 764                 case EV_ENTRY:
 765                         robot.use_left_bumper = true;
 766                         robot.use_right_bumper = true;
 767                         robot.use_back_bumpers = true;
 768                         robot.ignore_hokuyo = true;
 769
 770                         robot_trajectory_new(&tcSlow);
 771                         robot_trajectory_add_final_point_trans(
 772                                 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + load_dist,
 773                                 0.29 + 3*0.28,
 774                                 ARRIVE_FROM(DEG2RAD(180), 0.20));
 775                         break;
 776                 case EV_MOTION_DONE:
 777                         FSM_TIMER(1000);
 778                         act_jaws(CLOSE);
 779                         break;
 780                 case EV_TIMER:
 781                         FSM_TRANSITION(go_out_fourth_green_figure);
 782                         break;
 783                 case EV_RETURN:
 784                 case EV_MOTION_ERROR:
 785                 case EV_EXIT:
 786                         break;
 787         }
 788 }
 789
 790 FSM_STATE(go_out_fourth_green_figure)
 791 {
 792         switch(FSM_EVENT) {
 793                 case EV_ENTRY:
 794                         robot.use_left_bumper = true;
 795                         robot.use_right_bumper = true;
 796                         robot.use_back_bumpers = true;
 797                         robot.ignore_hokuyo = true;
 798
 799                         robot_trajectory_new_backward(&tcFast);
 800                         robot_trajectory_add_final_point_trans(
 801                                 0.45 + 0.175,
 802                                 3*0.35 + 0.175,
 803                                 NO_TURN());
 804                         break;
 805                 case EV_MOTION_DONE:
 806                         FSM_TRANSITION(place_figure_to_red_square);
 807                         break;
 808                 case EV_TIMER:
 809                         break;
 810                 case EV_START:
 811                 case EV_RETURN:
 812                 case EV_MOTION_ERROR:
 813                 case EV_EXIT:
 814                         break;
 815         }
 816 }
 817
 818 FSM_STATE(place_figure_to_red_square)
 819 {
 820         switch(FSM_EVENT) {
 821                 case EV_ENTRY:
 822                         robot.use_left_bumper = true;
 823                         robot.use_right_bumper = true;
 824                         robot.use_back_bumpers = true;
 825                         robot.ignore_hokuyo = false;
 826                         robot_trajectory_new(&tcSlow);
 827                         robot_trajectory_add_final_point_trans(
 828                                 0.45 + 0.175,
 829                                 0.7 + 0.15 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
 830                                 ARRIVE_FROM(DEG2RAD(-90), 0.05));
 831                         break;
 832                 case EV_START:
 833                 case EV_TIMER:
 834                         FSM_TRANSITION(leave_red_square_figure);
 835                         break;
 836                 case EV_RETURN:
 837                 case EV_MOTION_DONE:
 838                         act_jaws(OPEN);
 839                         FSM_TIMER(1000);
 840                         break;
 841                 case EV_MOTION_ERROR:
 842                 case EV_SWITCH_STRATEGY:
 843                         DBG_PRINT_EVENT("unhandled event");
 844                 case EV_EXIT:
 845                         break;
 846         }
 847 }
 848
 849 FSM_STATE(leave_red_square_figure)
 850 {
 851         switch(FSM_EVENT) {
 852                 case EV_ENTRY:
 853                         robot.use_left_bumper = true;
 854                         robot.use_right_bumper = true;
 855                         robot.use_back_bumpers = true;
 856                         robot.ignore_hokuyo = false;
 857
 858                         robot_trajectory_new_backward(&tcFast);
 859 //                      robot_trajectory_add_point_trans(
 860 //                              0.45 + 0.175,
 861 //                              0.7 + 0.7);
 862                         robot_trajectory_add_final_point_trans(
 863                                 0.45 + 0.175,
 864                                 0.7 + 0.7,
 865                                 NO_TURN());
 866                         break;
 867                 case EV_START:
 868                 case EV_TIMER:
 869                         break;
 870                 case EV_RETURN:
 871                 case EV_MOTION_DONE:
 872                         act_jaws(CLOSE);
 873                         SUBFSM_RET(NULL);
 874                         break;
 875                 case EV_MOTION_ERROR:
 876                 case EV_EXIT:
 877                         break;
 878         }
 879 }
 880
 881 /** pick fifth green figure from green area */
 882 FSM_STATE(approach_fifth_green_figure)
 883 {
 884         switch(FSM_EVENT) {
 885                 case EV_ENTRY:
 886                         robot.use_left_bumper = true;
 887                         robot.use_right_bumper = true;
 888                         robot.use_back_bumpers = true;
 889                         robot.ignore_hokuyo = true;
 890
 891                         robot_trajectory_new(&tcFast);
 892                         robot_trajectory_add_final_point_trans(
 893                                 0.45 + 0.3 - app_dist,
 894                                 0.29 + 4*0.28,
 895                                 TURN(DEG2RAD(180)));
 896                         break;
 897                 case EV_MOTION_DONE:
 898                         act_jaws(OPEN);
 899                         FSM_TIMER(1000);
 900                         break;
 901                 case EV_TIMER:
 902                         FSM_TRANSITION(load_fifth_green_figure);
 903                         break;
 904                 case EV_RETURN:
 905                 case EV_MOTION_ERROR:
 906                 case EV_EXIT:
 907                         break;
 908         }
 909 }
 910
 911 FSM_STATE(load_fifth_green_figure)
 912 {
 913         switch(FSM_EVENT) {
 914                 case EV_ENTRY:
 915                         robot.use_left_bumper = true;
 916                         robot.use_right_bumper = true;
 917                         robot.use_back_bumpers = true;
 918                         robot.ignore_hokuyo = true;
 919
 920                         robot_trajectory_new(&tcSlow);
 921                         robot_trajectory_add_final_point_trans(
 922                                 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + load_dist,
 923                                 0.29 + 4*0.28,
 924                                 ARRIVE_FROM(DEG2RAD(180), 0.20));
 925                         break;
 926                 case EV_MOTION_DONE:
 927                         FSM_TIMER(1000);
 928                         act_jaws(CLOSE);
 929                         break;
 930                 case EV_TIMER:
 931                         FSM_TRANSITION(go_out_fifth_green_figure);
 932                         break;
 933                 case EV_RETURN:
 934                 case EV_MOTION_ERROR:
 935                 case EV_EXIT:
 936                         break;
 937         }
 938 }
 939
 940 FSM_STATE(go_out_fifth_green_figure)
 941 {
 942         switch(FSM_EVENT) {
 943                 case EV_ENTRY:
 944                         robot.use_left_bumper = true;
 945                         robot.use_right_bumper = true;
 946                         robot.use_back_bumpers = true;
 947                         robot.ignore_hokuyo = true;
 948
 949                         robot_trajectory_new_backward(&tcFast);
 950                         robot_trajectory_add_final_point_trans(
 951                                 0.45 + 2*0.35 + 0.175,
 952                                 4*0.35,
 953                                 NO_TURN());
 954                         break;
 955                 case EV_MOTION_DONE:
 956                         break;
 957                 case EV_TIMER:
 958                         break;
 959                 case EV_START:
 960                 case EV_RETURN:
 961                 case EV_MOTION_ERROR:
 962                 case EV_EXIT:
 963                         break;
 964         }
 965 }
 966
 967 /** pick center figure */
 968 FSM_STATE(approach_center_figure)
 969 {
 970         switch(FSM_EVENT) {
 971                 case EV_ENTRY:
 972                         robot.use_left_bumper = true;
 973                         robot.use_right_bumper = true;
 974                         robot.use_back_bumpers = false;
 975                         robot.ignore_hokuyo = false;
 976
 977                         robot_trajectory_new(&tcVeryFast);
 978                         robot_trajectory_add_point_trans(
 979                               0.45 + 0.35,
 980                               5*0.35 + 0.175);
 981                         robot_trajectory_add_final_point_trans(
 982                                 0.45 + 3*0.35,
 983                                 4*0.35 + 0.1,
 984                                 TURN(DEG2RAD(-90)));
 985                         FSM_TIMER(2000);
 986                         break;
 987                 case EV_START:
 988                 case EV_TIMER:
 989                         robot.use_back_bumpers = true;
 990                         break;
 991                 case EV_RETURN:
 992                 case EV_MOTION_DONE:
 993                         FSM_TRANSITION(load_center_figure);
 994                         break;
 995                 case EV_MOTION_ERROR:
 996                 case EV_EXIT:
 997                         break;
 998         }
 999 }
1000
1001 FSM_STATE(load_center_figure)
1002 {
1003         switch(FSM_EVENT) {
1004                 case EV_ENTRY:
1005                         act_jaws(OPEN);
1006                         robot.use_left_bumper = true;
1007                         robot.use_right_bumper = true;
1008                         robot.use_back_bumpers = true;
1009                         robot.ignore_hokuyo = false;
1010
1011                         robot_trajectory_new(&tcFast);
1012                         robot_trajectory_add_final_point_trans(
1013                                 0.45 + 3*0.35,
1014                                 3*0.35 + 0.1,
1015                                 ARRIVE_FROM(DEG2RAD(-90), 0.1));
1016                         break;
1017                 case EV_START:
1018                 case EV_TIMER:
1019                         break;
1020                 case EV_RETURN:
1021                 case EV_MOTION_DONE:
1022                         act_jaws(CLOSE);
1023                         FSM_TRANSITION(place_figure_to_bonus_area);
1024                         break;
1025                 case EV_MOTION_ERROR:
1026                 case EV_EXIT:
1027                         break;
1028         }
1029 }