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 <trgen.h>
  16 #include "match-timing.h"
  17 #include <stdbool.h>
  18 #include <ul_log.h>
  19
  20 UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
  21
  22 #include "common-states.h"
  23
  24 /************************************************************************
  25  * Functions used in and called from all the (almost identical)
  26  * "wait for start" states in particular strategies.
  27  ************************************************************************/
  28
  29 #undef DBG_FSM_STATE
  30 #define DBG_FSM_STATE(name)     do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
  31                                                                    fsm->debug_name, robot_current_time(), \
  32                                                                    name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
  33
  34
  35 void set_initial_position()
  36 {
  37         robot_set_est_pos_trans(ROBOT_START_X_M,
  38                                 ROBOT_START_Y_M,
  39                                 DEG2RAD(ROBOT_START_ANGLE_DEG));
  40 }
  41
  42 void actuators_home()
  43 {
  44         act_jaws(CLOSE);
  45         // drive lift to home position
  46         //act_lift(0, 0, 1);
  47         // unset the homing request
  48         //act_lift(0, 0, 0);
  49 }
  50
  51 void start_entry()
  52 {
  53         pthread_t thid;
  54         robot.check_turn_safety = false;
  55         pthread_create(&thid, NULL, timing_thread, NULL);
  56         start_timer();
  57 }
  58
  59 // We set initial position periodically in order for it to be updated
  60 // on the display if the team color is changed during waiting for
  61 // start.
  62 void start_timer()
  63 {
  64         set_initial_position();
  65         if (robot.start_state == START_PLUGGED_IN)
  66                 actuators_home();
  67 }
  68
  69 void start_go()
  70 {
  71         sem_post(&robot.start);
  72         actuators_home();
  73         set_initial_position();
  74 }
  75
  76 void start_exit()
  77 {
  78
  79 }
  80
  81 void inline enable_bumpers(bool enabled)
  82 {
  83         robot.use_left_bumper = enabled;
  84         robot.use_right_bumper = enabled;
  85         robot.use_back_bumpers = enabled;
  86 }
  87
  88 void enable_my_square_walls(bool enabled)
  89 {
  90         for (int i = 0; i < 15; i++) {
  91                 if (robot.team_color == RED)
  92                         ShmapSetCircleFlag(red_sq[i].x, red_sq[i].y, 0.2, enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
  93                 else
  94                         ShmapSetCircleFlag(blue_sq[i].x, blue_sq[i].y, 0.2,enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
  95         }
  96
  97         //if (robot.team_color == RED)
  98                 //ShmapSetRectangleFlag(0.45, 0, 0.45 + 0.35 + 0.2, PLAYGROUND_HEIGHT_M, enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
  99         //else
 100                 //ShmapSetRectangleFlag(PLAYGROUND_WIDTH_M - 0.45 - 0.35 - 0.2, 0, PLAYGROUND_WIDTH_M - 0.45, PLAYGROUND_HEIGHT_M, enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
 101
 102 }
 103
 104 /************************************************************************
 105  * Trajectory constraints used; They are  initialized in the main() function in competition.cc
 106  ************************************************************************/
 107
 108 struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow;
 109
 110 /** generate random positions on oponent squares and goto this position */
 111 FSM_STATE(move_around)
 112 {
 113         int goal;
 114         double goal_x, goal_y;
 115         static bool entry = true;
 116
 117         switch(FSM_EVENT) {
 118                 case EV_ENTRY:
 119                         robot.use_left_bumper = true;
 120                         robot.use_right_bumper = true;
 121                         robot.use_back_bumpers = true;
 122                         robot.ignore_hokuyo = false;
 123
 124                         act_jaws(CLOSE);
 125
 126                         enable_my_square_walls(true);
 127
 128                         srand((int)(robot_current_time()*1000));
 129                         goal = (rand() % (SQ_CNTR - 5)) + 5;
 130                         printf("goal %d time %f\n", goal, robot_current_time());
 131                         if (robot.team_color == RED) {
 132                                 goal_x = blue_sq[goal].x;
 133                                 goal_y = blue_sq[goal].y;
 134                         } else {
 135                                 goal_x = red_sq[goal].x;
 136                                 goal_y = red_sq[goal].y;
 137                         }
 138
 139                         robot_goto_notrans(goal_x, goal_y, NO_TURN(), &tcFast);
 140                         break;
 141                 case EV_START:
 142                 case EV_TIMER:
 143                         break;
 144                 case EV_RETURN:
 145                 case EV_MOTION_DONE:
 146                         SUBFSM_RET(NULL);
 147                         break;
 148                 case EV_MOTION_ERROR:
 149                 case EV_EXIT:
 150                         break;
 151         }
 152 }
 153
 154 /** securely bypass firt figure in front of starting area */
 155 FSM_STATE(bypass_figure_in_front_of_start)
 156 {
 157         switch(FSM_EVENT) {
 158                 case EV_ENTRY:
 159                         robot.use_left_bumper = true;
 160                         robot.use_right_bumper = true;
 161                         robot.use_back_bumpers = true;
 162                         robot.ignore_hokuyo = false;
 163
 164                         robot_trajectory_new(&tcFast);
 165                         robot_trajectory_add_point_trans(
 166                                 0.45 + 0.3,
 167                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
 168                         robot_trajectory_add_final_point_trans(
 169                                 0.45 + 0.3,
 170                                 4*0.35,
 171                                 NO_TURN());
 172                         break;
 173                 case EV_MOTION_DONE:
 174                         SUBFSM_RET(NULL);
 175                         break;
 176                 case EV_START:
 177                 case EV_TIMER:
 178                 case EV_RETURN:
 179                 case EV_MOTION_ERROR:
 180                 case EV_SWITCH_STRATEGY:
 181                         DBG_PRINT_EVENT("unhandled event");
 182                 case EV_EXIT:
 183                         break;
 184         }
 185 }
 186
 187 /** pick second figure from green area */
 188 FSM_STATE(approach_second_green_figure)
 189 {
 190         switch(FSM_EVENT) {
 191                 case EV_ENTRY:
 192                         robot.use_left_bumper = true;
 193                         robot.use_right_bumper = true;
 194                         robot.use_back_bumpers = true;
 195                         robot.ignore_hokuyo = false;
 196
 197                         robot_trajectory_new(&tcFast);
 198                         robot_trajectory_add_final_point_trans(
 199                                 0.45 + 0.3,
 200                                 0.29 + 0.28,
 201                                 TURN(DEG2RAD(180)));
 202                         break;
 203                 case EV_MOTION_DONE:
 204                         act_jaws(OPEN);
 205                         FSM_TIMER(1000);
 206                         break;
 207                 case EV_TIMER:
 208                         FSM_TRANSITION(load_second_green_figure);
 209                         break;
 210                 case EV_RETURN:
 211                 case EV_MOTION_ERROR:
 212                 case EV_EXIT:
 213                         break;
 214         }
 215 }
 216
 217 FSM_STATE(load_second_green_figure)
 218 {
 219         switch(FSM_EVENT) {
 220                 case EV_ENTRY:
 221                         robot.use_left_bumper = true;
 222                         robot.use_right_bumper = true;
 223                         robot.use_back_bumpers = true;
 224                         robot.ignore_hokuyo = true;
 225
 226                         robot_trajectory_new(&tcSlow);
 227                         robot_trajectory_add_final_point_trans(
 228                                 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + 0.04,
 229                                 0.29 + 0.28,
 230                                 ARRIVE_FROM(DEG2RAD(180), 0.10));
 231                         break;
 232                 case EV_MOTION_DONE:
 233                         FSM_TIMER(1000);
 234                         act_jaws(CATCH);
 235                         break;
 236                 case EV_TIMER:
 237                         FSM_TRANSITION(go_out_second_green_figure);
 238                         break;
 239                 case EV_RETURN:
 240                 case EV_MOTION_ERROR:
 241                 case EV_EXIT:
 242                         break;
 243         }
 244 }
 245
 246 FSM_STATE(go_out_second_green_figure)
 247 {
 248         switch(FSM_EVENT) {
 249                 case EV_ENTRY:
 250                         robot.use_left_bumper = true;
 251                         robot.use_right_bumper = true;
 252                         robot.use_back_bumpers = true;
 253                         robot.ignore_hokuyo = true;
 254
 255                         robot_trajectory_new_backward(&tcFast);
 256                         robot_trajectory_add_final_point_trans(
 257                                 0.45 + 0.3,
 258                                 0.7,
 259                                 NO_TURN());
 260                         break;
 261                 case EV_MOTION_DONE:
 262                 case EV_TIMER:
 263                         FSM_TRANSITION(place_figure_to_protected_block);
 264                         break;
 265                 case EV_START:
 266                 case EV_RETURN:
 267                 case EV_MOTION_ERROR:
 268                 case EV_EXIT:
 269                         break;
 270         }
 271 }
 272
 273 FSM_STATE(place_figure_to_protected_block)
 274 {
 275         switch(FSM_EVENT) {
 276                 case EV_ENTRY:
 277                         robot.use_left_bumper = true;
 278                         robot.use_right_bumper = true;
 279                         robot.use_back_bumpers = true;
 280                         robot.ignore_hokuyo = false;
 281
 282                         robot_trajectory_new(&tcSlow);
 283                         robot_trajectory_add_final_point_trans(
 284                                 0.45 + 0.3,
 285                                 0.12 + 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
 286                                 ARRIVE_FROM(DEG2RAD(-90), 0.20));
 287                         break;
 288                 case EV_START:
 289                 case EV_TIMER:
 290                         FSM_TRANSITION(leave_protected_figure);
 291                         break;
 292                 case EV_RETURN:
 293                 case EV_MOTION_DONE:
 294                         act_jaws(OPEN);
 295                         FSM_TIMER(1000);
 296                         break;
 297                 case EV_MOTION_ERROR:
 298                 case EV_SWITCH_STRATEGY:
 299                         DBG_PRINT_EVENT("unhandled event");
 300                 case EV_EXIT:
 301                         break;
 302         }
 303 }
 304
 305 FSM_STATE(leave_protected_figure)
 306 {
 307         switch(FSM_EVENT) {
 308                 case EV_ENTRY:
 309                         robot.use_left_bumper = true;
 310                         robot.use_right_bumper = true;
 311                         robot.use_back_bumpers = true;
 312                         robot.ignore_hokuyo = false;
 313
 314                         robot_trajectory_new_backward(&tcFast);
 315                         robot_trajectory_add_point_trans(
 316                                 0.45 + 0.3,
 317                                 0.7);
 318                         robot_trajectory_add_final_point_trans(
 319                                 0.45 + 0.3,
 320                                 0.29 + 2*0.28,
 321                                 NO_TURN());
 322                         break;
 323                 case EV_START:
 324                 case EV_TIMER:
 325                         break;
 326                 case EV_RETURN:
 327                 case EV_MOTION_DONE:
 328                         act_jaws(CLOSE);
 329                         SUBFSM_RET(NULL);
 330                         break;
 331                 case EV_MOTION_ERROR:
 332                 case EV_EXIT:
 333                         break;
 334         }
 335 }
 336
 337 /** pick third figure from green area */
 338 FSM_STATE(approach_third_green_figure)
 339 {
 340         switch(FSM_EVENT) {
 341                 case EV_ENTRY:
 342                         robot.use_left_bumper = true;
 343                         robot.use_right_bumper = true;
 344                         robot.use_back_bumpers = true;
 345                         robot.ignore_hokuyo = false;
 346
 347                         robot_trajectory_new(&tcFast);
 348                         robot_trajectory_add_final_point_trans(
 349                                 0.45 + 0.3,
 350                                 0.29 + 2*0.28,
 351                                 TURN(DEG2RAD(180)));
 352                         break;
 353                 case EV_MOTION_DONE:
 354                         act_jaws(OPEN);
 355                         FSM_TIMER(1000);
 356                         break;
 357                 case EV_TIMER:
 358                         FSM_TRANSITION(load_third_green_figure);
 359                         break;
 360                 case EV_RETURN:
 361                 case EV_MOTION_ERROR:
 362                 case EV_EXIT:
 363                         break;
 364         }
 365 }
 366
 367 FSM_STATE(load_third_green_figure)
 368 {
 369         switch(FSM_EVENT) {
 370                 case EV_ENTRY:
 371                         robot.use_left_bumper = true;
 372                         robot.use_right_bumper = true;
 373                         robot.use_back_bumpers = true;
 374                         robot.ignore_hokuyo = false;
 375
 376                         robot_trajectory_new(&tcSlow);
 377                         robot_trajectory_add_final_point_trans(
 378                                 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + 0.04,
 379                                 0.29 + 2*0.28,
 380                                 ARRIVE_FROM(DEG2RAD(180), 0.20));
 381                         break;
 382                 case EV_MOTION_DONE:
 383                         FSM_TIMER(1000);
 384                         act_jaws(CATCH);
 385                         break;
 386                 case EV_TIMER:
 387                         FSM_TRANSITION(go_out_third_green_figure);
 388                         break;
 389                 case EV_RETURN:
 390                 case EV_MOTION_ERROR:
 391                 case EV_EXIT:
 392                         break;
 393         }
 394 }
 395
 396 FSM_STATE(go_out_third_green_figure)
 397 {
 398         switch(FSM_EVENT) {
 399                 case EV_ENTRY:
 400                         robot.use_left_bumper = true;
 401                         robot.use_right_bumper = true;
 402                         robot.use_back_bumpers = true;
 403                         robot.ignore_hokuyo = false;
 404
 405                         robot_trajectory_new_backward(&tcFast);
 406                         robot_trajectory_add_final_point_trans(
 407                                 0.45 + 0.35,
 408                                 0.29 + 2*0.28,
 409                                 NO_TURN());
 410                         break;
 411                 case EV_MOTION_DONE:
 412                 case EV_TIMER:
 413                         FSM_TRANSITION(place_figure_to_bonus_area);
 414                         break;
 415                 case EV_START:
 416                 case EV_RETURN:
 417                 case EV_MOTION_ERROR:
 418                 case EV_EXIT:
 419                         robot.use_left_bumper = true;
 420                         robot.use_right_bumper = true;
 421                         robot.ignore_hokuyo = false;
 422                         break;
 423         }
 424 }
 425
 426 FSM_STATE(place_figure_to_bonus_area)
 427 {
 428         switch(FSM_EVENT) {
 429                 case EV_ENTRY:
 430                         robot.use_left_bumper = true;
 431                         robot.use_right_bumper = true;
 432                         robot.use_back_bumpers = true;
 433                         robot.ignore_hokuyo = false;
 434
 435                         robot_trajectory_new(&tcSlow);
 436                         robot_trajectory_add_final_point_trans(
 437                                 0.45 + 0.7 + 0.2,
 438                                 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
 439                                 ARRIVE_FROM(DEG2RAD(-90), 0.20));
 440                         break;
 441                 case EV_START:
 442                 case EV_TIMER:
 443                         FSM_TRANSITION(leave_bonus_figure);
 444                         break;
 445                 case EV_RETURN:
 446                 case EV_MOTION_DONE:
 447                         act_jaws(OPEN);
 448                         FSM_TIMER(1000);
 449                         break;
 450                 case EV_MOTION_ERROR:
 451                 case EV_SWITCH_STRATEGY:
 452                         DBG_PRINT_EVENT("unhandled event");
 453                 case EV_EXIT:
 454                         break;
 455         }
 456 }
 457
 458 FSM_STATE(leave_bonus_figure)
 459 {
 460         switch(FSM_EVENT) {
 461                 case EV_ENTRY:
 462                         robot.use_left_bumper = true;
 463                         robot.use_right_bumper = true;
 464                         robot.use_back_bumpers = true;
 465                         robot.ignore_hokuyo = false;
 466
 467                         robot_trajectory_new_backward(&tcFast);
 468                         robot_trajectory_add_final_point_trans(
 469                                 0.45 + 0.7 + 0.2,
 470                                 0.7,
 471                                 NO_TURN());
 472                         break;
 473                 case EV_START:
 474                 case EV_TIMER:
 475                         break;
 476                 case EV_RETURN:
 477                 case EV_MOTION_DONE:
 478                         act_jaws(CLOSE);
 479                         SUBFSM_RET(NULL);
 480                         break;
 481                 case EV_MOTION_ERROR:
 482                 case EV_EXIT:
 483                         break;
 484         }
 485 }
 486
 487 /** pick fourth green figure from green area */
 488 FSM_STATE(approach_fourth_green_figure)
 489 {
 490         switch(FSM_EVENT) {
 491                 case EV_ENTRY:
 492                         robot.use_left_bumper = true;
 493                         robot.use_right_bumper = true;
 494                         robot.use_back_bumpers = true;
 495                         robot.ignore_hokuyo = false;
 496
 497                         robot_trajectory_new(&tcFast);
 498                         robot_trajectory_add_final_point_trans(
 499                                 0.45 + 0.3,
 500                                 0.29 + 3*0.28,
 501                                 TURN(DEG2RAD(180)));
 502                         break;
 503                 case EV_MOTION_DONE:
 504                         act_jaws(OPEN);
 505                         FSM_TIMER(1000);
 506                         break;
 507                 case EV_TIMER:
 508                         FSM_TRANSITION(load_fourth_green_figure);
 509                         break;
 510                 case EV_RETURN:
 511                 case EV_MOTION_ERROR:
 512                 case EV_EXIT:
 513                         break;
 514         }
 515 }
 516
 517 FSM_STATE(load_fourth_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 = true;
 525
 526                         robot_trajectory_new(&tcSlow);
 527                         robot_trajectory_add_final_point_trans(
 528                                 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + 0.03,
 529                                 0.29 + 3*0.28,
 530                                 ARRIVE_FROM(DEG2RAD(180), 0.20));
 531                         break;
 532                 case EV_MOTION_DONE:
 533                         FSM_TIMER(1000);
 534                         act_jaws(CATCH);
 535                         break;
 536                 case EV_TIMER:
 537                         FSM_TRANSITION(go_out_fourth_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(go_out_fourth_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_backward(&tcFast);
 556                         robot_trajectory_add_final_point_trans(
 557                                 0.45 + 0.175,
 558                                 3*0.35 + 0.175,
 559                                 NO_TURN());
 560                         break;
 561                 case EV_MOTION_DONE:
 562                 case EV_TIMER:
 563                         FSM_TRANSITION(place_figure_to_red_square);
 564                         break;
 565                 case EV_START:
 566                 case EV_RETURN:
 567                 case EV_MOTION_ERROR:
 568                 case EV_EXIT:
 569                         break;
 570         }
 571 }
 572
 573 FSM_STATE(place_figure_to_red_square)
 574 {
 575         switch(FSM_EVENT) {
 576                 case EV_ENTRY:
 577                         robot.use_left_bumper = true;
 578                         robot.use_right_bumper = true;
 579                         robot.use_back_bumpers = true;
 580                         robot.ignore_hokuyo = false;
 581                         robot_trajectory_new(&tcSlow);
 582                         robot_trajectory_add_final_point_trans(
 583                                 0.45 + 0.175,
 584                                 0.7 + 0.175 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
 585                                 ARRIVE_FROM(DEG2RAD(-90), 0.05));
 586                         break;
 587                 case EV_START:
 588                 case EV_TIMER:
 589                         FSM_TRANSITION(leave_red_square_figure);
 590                         break;
 591                 case EV_RETURN:
 592                 case EV_MOTION_DONE:
 593                         act_jaws(OPEN);
 594                         FSM_TIMER(1000);
 595                         break;
 596                 case EV_MOTION_ERROR:
 597                 case EV_SWITCH_STRATEGY:
 598                         DBG_PRINT_EVENT("unhandled event");
 599                 case EV_EXIT:
 600                         break;
 601         }
 602 }
 603
 604 FSM_STATE(leave_red_square_figure)
 605 {
 606         switch(FSM_EVENT) {
 607                 case EV_ENTRY:
 608                         robot.use_left_bumper = true;
 609                         robot.use_right_bumper = true;
 610                         robot.use_back_bumpers = true;
 611                         robot.ignore_hokuyo = false;
 612
 613                         robot_trajectory_new_backward(&tcFast);
 614 //                      robot_trajectory_add_point_trans(
 615 //                              0.45 + 0.175,
 616 //                              0.7 + 0.7);
 617                         robot_trajectory_add_final_point_trans(
 618                                 0.45 + 0.175,
 619                                 0.7 + 0.7,
 620                                 NO_TURN());
 621                         break;
 622                 case EV_START:
 623                 case EV_TIMER:
 624                         break;
 625                 case EV_RETURN:
 626                 case EV_MOTION_DONE:
 627                         act_jaws(CLOSE);
 628                         SUBFSM_RET(NULL);
 629                         break;
 630                 case EV_MOTION_ERROR:
 631                 case EV_EXIT:
 632                         break;
 633         }
 634 }