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 "corns_configs.h"
  15 #include "actuators.h"
  16 #include <trgen.h>
  17 #include "match-timing.h"
  18 #include "eb2010misc.h"
  19 #include <stdbool.h>
  20 #include <ul_log.h>
  21
  22 UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
  23
  24 #include "common-states.h"
  25
  26 /************************************************************************
  27  * Functions used in and called from all the (almost identical)
  28  * "wait for start" states in particular strategies.
  29  ************************************************************************/
  30
  31 #undef DBG_FSM_STATE
  32 #define DBG_FSM_STATE(name)     do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
  33                                                                    fsm->debug_name, robot_current_time(), \
  34                                                                    name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
  35
  36
  37 static void set_initial_position()
  38 {
  39         robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
  40                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
  41                                 DEG2RAD(180));
  42 }
  43
  44 static void actuators_home()
  45 {
  46         static int tmp = 0;
  47         act_vidle(VIDLE_UP - tmp, VIDLE_FAST_SPEED);
  48         tmp = 1 - tmp;          // Force movement (we need to change the target position)
  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         act_camera_on();
  58 }
  59
  60 // We set initial position periodically in order for it to be updated
  61 // on the display if the team color is changed during waiting for
  62 // start.
  63 void start_timer()
  64 {
  65         set_initial_position();
  66         if (robot.start_state == START_PLUGGED_IN)
  67                 actuators_home();
  68 }
  69
  70 void start_go()
  71 {
  72         sem_post(&robot.start);
  73         actuators_home();
  74         set_initial_position();
  75 }
  76
  77 void start_exit()
  78 {
  79         robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center);
  80         act_camera_off();
  81 }
  82
  83 /************************************************************************
  84  * Trajectory constraints used; They are  initialized in the main() function in competition.cc
  85  ************************************************************************/
  86
  87 struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow;
  88
  89 #define VIDLE_TIMEOUT 2000
  90
  91 /************************************************************************
  92  * States that form the "collect some oranges" subautomaton. Calling automaton
  93  * SHOULD ALWAYS call the "approach_the_slope" state.
  94  ************************************************************************/
  95
  96 bool inline is_ready_to_climb_the_slope(enum which_side which_slope, double x, double y) {
  97         bool ret;
  98         if (which_slope == MINE) {
  99                 ret = x < 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5;
 100         } else if (which_slope == OPPONENTS) {
 101                 ret = x > 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5;
 102         } else {
 103                 ul_logfatal("ERROR: unknown side;");
 104 #warning Remove the next line
 105                 robot_exit();
 106         }
 107         return ret;
 108 }
 109
 110 static struct slope_approach_style *slope_approach_style_p;
 111
 112 /* assures that the robot is near the slope rim; if outside our starting area, approach the slope first */
 113 FSM_STATE(approach_the_slope)
 114 {
 115         switch(FSM_EVENT) {
 116                 case EV_ENTRY: {
 117                                 slope_approach_style_p = (struct slope_approach_style *) FSM_EVENT_PTR;
 118                                 if (slope_approach_style_p == NULL) {
 119                                         ul_logfatal("\n\nit is not allowed to call the approach_the_slope state  with NULL data!!\n\n");
 120 #warning remove the next line
 121                                         robot_exit();
 122                                 }
 123                                 double x, y, phi;
 124                                 robot_get_est_pos_trans(&x, &y, &phi);
 125                                 /* decide */
 126                                 bool ready_to_climb_the_slope = is_ready_to_climb_the_slope(slope_approach_style_p->which_side, x, y);
 127                                 /* if necessary, approach the slope */
 128                                 if (ready_to_climb_the_slope) {
 129                                         FSM_TRANSITION(climb_the_slope);
 130                                 } else {
 131                                         robot_goto_trans(
 132                                                 x_coord(0.3, slope_approach_style_p->which_side),
 133                                                 PLAYGROUND_HEIGHT_M - ROBOT_WIDTH_M/2 - 0.03,
 134                                                 ARRIVE_FROM(DEG2RAD(0), 0.02),
 135                                                 &tcFast);
 136                                 }
 137                                 break;
 138                         }
 139                 case EV_MOTION_DONE:
 140                         FSM_TRANSITION(climb_the_slope);
 141                         break;
 142                 case EV_START:
 143                 case EV_TIMER:
 144                 case EV_RETURN:
 145                 case EV_VIDLE_DONE:
 146                 case EV_MOTION_ERROR:
 147                 case EV_SWITCH_STRATEGY:
 148                         DBG_PRINT_EVENT("unhandled event");
 149                 case EV_EXIT:
 150                         break;
 151         }
 152 }
 153
 154 void inline enable_switches(bool enabled)
 155 {
 156         robot.use_left_switch = enabled;
 157         robot.use_right_switch = enabled;
 158         robot.use_back_switch = enabled;
 159 }
 160
 161 FSM_STATE(climb_the_slope)
 162 {
 163         struct TrajectoryConstraints tc;
 164         switch(FSM_EVENT) {
 165                 case EV_ENTRY: {
 166                                 // disables using side switches on bumpers when going up
 167                                 enable_switches(false);
 168                                 robot.ignore_hokuyo = true;
 169                                 /* create the trajectory and go */
 170                                 tc = tcSlow;
 171                                 tc.maxacc = 0.4;
 172                                 robot_trajectory_new_backward(&tc);
 173                                 if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_BOUNDARY) {
 174                                         act_vidle(VIDLE_LOAD_PREPARE, 5);
 175                                         robot_trajectory_add_point_trans(
 176                                                 x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
 177                                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01);
 178                                         robot_trajectory_add_final_point_trans(
 179                                                 x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, slope_approach_style_p->which_side),
 180                                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
 181                                                 NO_TURN());
 182                                 } else if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_CENTER) {
 183                                         FSM_TIMER(3800);
 184                                         robot_trajectory_add_point_trans(
 185                                                 x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
 186                                                 //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85));
 187                                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199);
 188                                         robot_trajectory_add_final_point_trans(
 189                                                 x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, slope_approach_style_p->which_side),
 190                                                 //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85),
 191                                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199,
 192                                                 NO_TURN());
 193                                 }
 194                                 break;
 195                         }
 196                 case EV_MOTION_DONE:
 197                         SUBFSM_TRANSITION(load_oranges, NULL);
 198                         break;
 199                 case EV_RETURN:
 200                         FSM_TRANSITION(sledge_down);
 201                         break;
 202                 case EV_TIMER:
 203                         act_vidle(VIDLE_LOAD_PREPARE, 15);
 204                         break;
 205                 case EV_START:
 206                 case EV_VIDLE_DONE:
 207                 case EV_MOTION_ERROR:
 208                 case EV_SWITCH_STRATEGY:
 209                         DBG_PRINT_EVENT("unhandled event");
 210                 case EV_EXIT:
 211                         break;
 212         }
 213 }
 214
 215 /* subautomaton to load oranges in two stages */
 216 FSM_STATE_DECL(load_oranges2);
 217 FSM_STATE_DECL(load_oranges3);
 218 FSM_STATE(load_oranges)
 219 {
 220         switch(FSM_EVENT) {
 221                 case EV_ENTRY:
 222                         FSM_TIMER(1000);
 223                         act_vidle(VIDLE_MIDDLE, VIDLE_MEDIUM_SPEED);
 224                         break;
 225                 case EV_VIDLE_DONE:
 226                         FSM_TIMER(1000);
 227                         break;
 228                 case EV_TIMER:
 229                         FSM_TRANSITION(load_oranges2);
 230                         break;
 231                 case EV_MOTION_DONE:
 232                 case EV_START:
 233                 case EV_RETURN:
 234                 case EV_MOTION_ERROR:
 235                 case EV_SWITCH_STRATEGY:
 236                         DBG_PRINT_EVENT("unhandled event");
 237                 case EV_EXIT:
 238                         break;
 239         }
 240 }
 241
 242 FSM_STATE(load_oranges2)
 243 {
 244         switch(FSM_EVENT) {
 245                 case EV_ENTRY:
 246                         act_vidle(VIDLE_UP, VIDLE_MEDIUM_SPEED);
 247                         FSM_TIMER(1000);
 248                         break;
 249                 case EV_VIDLE_DONE:
 250                         FSM_TRANSITION(load_oranges3);
 251                         //SUBFSM_RET(NULL);
 252                         break;
 253                 case EV_TIMER:
 254                         FSM_TRANSITION(load_oranges3);
 255                         //SUBFSM_RET(NULL);
 256                         break;
 257                 case EV_MOTION_DONE:
 258                 case EV_START:
 259                 case EV_RETURN:
 260                 case EV_MOTION_ERROR:
 261                 case EV_SWITCH_STRATEGY:
 262                         DBG_PRINT_EVENT("unhandled event");
 263                 case EV_EXIT:
 264                         act_vidle(VIDLE_UP-1, VIDLE_FAST_SPEED);
 265                         break;
 266         }
 267 }
 268
 269 FSM_STATE(load_oranges3)
 270 {
 271         switch(FSM_EVENT) {
 272                 case EV_ENTRY:
 273                         act_vidle(VIDLE_MIDDLE+50, 0);
 274                         FSM_TIMER(500);
 275                         break;
 276                 case EV_VIDLE_DONE:
 277                         SUBFSM_RET(NULL);
 278                         break;
 279                 case EV_TIMER:
 280                         SUBFSM_RET(NULL);
 281                         break;
 282                 case EV_MOTION_DONE:
 283                 case EV_START:
 284                 case EV_RETURN:
 285                 case EV_MOTION_ERROR:
 286                 case EV_SWITCH_STRATEGY:
 287                         DBG_PRINT_EVENT("unhandled event");
 288                 case EV_EXIT:
 289                         act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
 290                         break;
 291         }
 292 }
 293
 294 FSM_STATE(sledge_down)
 295 {
 296         struct TrajectoryConstraints tc;
 297         switch(FSM_EVENT) {
 298                 case EV_ENTRY:
 299                         tc = tcFast;
 300                         tc.maxe = 0.5;
 301                         robot_trajectory_new(&tc);
 302
 303                         if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_BOUNDARY) {
 304                                 robot_trajectory_add_point_trans(
 305                                         x_coord(1.2 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
 306                                         PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01);
 307                                 robot_trajectory_add_point_trans(
 308                                         x_coord(1.0 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
 309                                         PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.01);
 310                                 robot_trajectory_add_point_trans(
 311                                         x_coord(0.8 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
 312                                         PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05);
 313                                 robot_trajectory_add_point_trans(
 314                                         x_coord(0.6 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
 315                                         PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
 316                                 robot_trajectory_add_final_point_trans(
 317                                         x_coord(0.5 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
 318                                         PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.19,
 319                                         NO_TURN());
 320                         } else if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_CENTER) {
 321                                 robot_trajectory_add_point_trans(
 322                                         x_coord(1 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
 323                                         //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85));
 324                                         PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199);
 325                                 robot_trajectory_add_final_point_trans(
 326                                         x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.2, slope_approach_style_p->which_side),
 327                                         //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85),
 328                                         PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199,
 329                                         NO_TURN());
 330                         }
 331                         break;
 332                 case EV_MOTION_DONE:
 333                         /* just for sure, try to close it one more time */
 334                         act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
 335                         SUBFSM_RET(NULL);
 336                         delete(slope_approach_style_p);
 337                         break;
 338                 case EV_START:
 339                 case EV_TIMER:
 340                 case EV_RETURN:
 341                 case EV_VIDLE_DONE:
 342                 case EV_MOTION_ERROR:
 343                 case EV_SWITCH_STRATEGY:
 344                         DBG_PRINT_EVENT("unhandled event");
 345                         break;
 346                 case EV_EXIT:
 347                         // enables using side switches on bumpers
 348                         enable_switches(true);
 349                         robot.ignore_hokuyo = false;
 350                         robot.check_turn_safety = true;
 351
 352                         break;
 353         }
 354 }
 355
 356 /************************************************************************
 357  * The "unload our oranges" subautomaton
 358  ************************************************************************/
 359
 360 FSM_STATE(to_cntainer_and_unld)
 361 {
 362         TrajectoryConstraints tcNotSoFast = trajectoryConstraintsDefault;
 363         tcNotSoFast.maxv = 0.5; tcNotSoFast.maxacc = 0.3; tcNotSoFast.maxomega = 2;
 364         switch(FSM_EVENT) {
 365                 case EV_ENTRY:
 366                         robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcNotSoFast);
 367                         break;
 368                 case EV_MOTION_DONE:
 369                         FSM_TIMER(1000); // FIXME: test this
 370                         act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
 371                         break;
 372                 case EV_TIMER:
 373                         FSM_TRANSITION(jerk);
 374                         break;
 375                 case EV_START:
 376                 case EV_RETURN:
 377                 case EV_VIDLE_DONE:
 378                 case EV_MOTION_ERROR:
 379                 case EV_SWITCH_STRATEGY:
 380                         DBG_PRINT_EVENT("unhandled event");
 381                 case EV_EXIT:
 382                         break;
 383         }
 384 }
 385
 386 FSM_STATE(jerk)
 387 {
 388         static char move_cnt;
 389         switch(FSM_EVENT) {
 390                 case EV_ENTRY:
 391                         move_cnt = 0;
 392                         robot_move_by(-0.05, NO_TURN(), &tcJerk);
 393                         break;
 394                 case EV_MOTION_DONE:
 395                         if (move_cnt == 0) {
 396                                 robot_move_by(0.05, NO_TURN(), &tcJerk);
 397                         } else if (move_cnt > 0) {
 398                                 FSM_TIMER(1500); // FIXME: test this
 399                         }
 400                         move_cnt++;
 401                         break;
 402                 case EV_TIMER:
 403                         act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
 404                         SUBFSM_RET(NULL);
 405                         break;
 406                 case EV_START:
 407                 case EV_RETURN:
 408                 case EV_VIDLE_DONE:
 409                 case EV_MOTION_ERROR:
 410                 case EV_SWITCH_STRATEGY:
 411                         DBG_PRINT_EVENT("unhandled event");
 412                 case EV_EXIT:
 413                         break;
 414         }
 415 }
 416
 417 /************************************************************************
 418  * The "collect corns" subautomaton
 419  ************************************************************************/
 420
 421 static enum where_to_go {
 422         CORN,
 423         TURN_AROUND,
 424         CONTAINER,
 425         NO_MORE_CORN
 426 } where_to_go = CORN;
 427
 428 static struct corn *corn_to_get;
 429
 430 FSM_STATE(rush_corns_decider)
 431 {
 432         switch(FSM_EVENT) {
 433                 case EV_ENTRY:
 434                         if (where_to_go == CORN) {
 435                                 FSM_TRANSITION(approach_next_corn);
 436                         } else if (where_to_go == CONTAINER) {
 437                                 FSM_TRANSITION(rush_the_corn);
 438                         } else if (where_to_go == TURN_AROUND) {
 439                                 FSM_TRANSITION(turn_around);
 440                         } else /* NO_MORE_CORN */ {
 441                         }
 442                         break;
 443                 case EV_START:
 444                 case EV_TIMER:
 445                 case EV_RETURN:
 446                 case EV_VIDLE_DONE:
 447                 case EV_MOTION_DONE:
 448                 case EV_MOTION_ERROR:
 449                 case EV_SWITCH_STRATEGY:
 450                         DBG_PRINT_EVENT("unhandled event");
 451                 case EV_EXIT:
 452                         break;
 453         }
 454 }
 455
 456 static int cnt = 0;
 457 FSM_STATE(approach_next_corn)
 458 {
 459         switch(FSM_EVENT) {
 460                 case EV_ENTRY: {
 461                                 double x, y, phi;
 462                                 robot_get_est_pos(&x, &y, &phi);
 463                                 ul_logdeb("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
 464                                         cnt, x, y, phi);
 465
 466                                 corn_to_get = choose_next_corn();
 467                                 if (corn_to_get) {
 468                                         Pos *p = get_corn_approach_position(corn_to_get);
 469                                         corn_to_get->was_collected = true;
 470                                         robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
 471                                         delete(p);
 472                                         where_to_go = CONTAINER;
 473                                 } else {
 474                                         where_to_go = NO_MORE_CORN;
 475                                 }
 476                                 break;
 477                         }
 478                 case EV_MOTION_DONE:
 479                         cnt++;
 480                         FSM_TRANSITION(rush_corns_decider);
 481                         break;
 482                 case EV_START:
 483                 case EV_TIMER:
 484                 case EV_RETURN:
 485                 case EV_VIDLE_DONE:
 486                 case EV_MOTION_ERROR:
 487                 case EV_SWITCH_STRATEGY:
 488                         DBG_PRINT_EVENT("unhandled event");
 489                 case EV_EXIT:
 490                         break;
 491         }
 492 }
 493
 494 FSM_STATE(rush_the_corn)
 495 {
 496         switch(FSM_EVENT) {
 497                 case EV_ENTRY:
 498                         double x;
 499                         if (robot.team_color == BLUE) {
 500                                 x = corn_to_get->position.x;
 501                         } else {
 502                                 x = PLAYGROUND_WIDTH_M - corn_to_get->position.x;
 503                         }
 504                         remove_wall_around_corn(x, corn_to_get->position.y);
 505                         robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcSlow);
 506                         where_to_go = TURN_AROUND;
 507                         break;
 508                 case EV_MOTION_DONE:
 509                         FSM_TRANSITION(rush_last_cms);
 510                         break;
 511                 case EV_START:
 512                 case EV_TIMER:
 513                 case EV_RETURN:
 514                 case EV_VIDLE_DONE:
 515                 case EV_MOTION_ERROR:
 516                 case EV_SWITCH_STRATEGY:
 517                         DBG_PRINT_EVENT("unhandled event");
 518                 case EV_EXIT:
 519                         break;
 520         }
 521 }
 522
 523 FSM_STATE(rush_last_cms)
 524 {
 525         static char move_cnt;
 526         switch(FSM_EVENT) {
 527                 case EV_ENTRY:
 528                         robot.ignore_hokuyo = true;
 529                         robot.check_turn_safety = false;
 530                         move_cnt = 0;
 531                         robot_move_by(0.08, NO_TURN(), &tcSlow);
 532                         break;
 533                 case EV_MOTION_DONE:
 534                         if (move_cnt == 0) {
 535                                 robot_move_by(-0.08, NO_TURN(), &tcSlow);
 536                         } else if (move_cnt > 0) {
 537                                 FSM_TRANSITION(rush_corns_decider);
 538                         }
 539                         move_cnt++;
 540                         break;
 541                 case EV_START:
 542                 case EV_TIMER:
 543                 case EV_RETURN:
 544                 case EV_VIDLE_DONE:
 545                 case EV_MOTION_ERROR:
 546                 case EV_SWITCH_STRATEGY:
 547                         DBG_PRINT_EVENT("unhandled event");
 548                 case EV_EXIT:
 549                         robot.ignore_hokuyo = false;
 550                         robot.check_turn_safety = true;
 551                         break;
 552         }
 553 }
 554
 555 // used to perform the maneuvre
 556 FSM_STATE(turn_around)
 557 {
 558         switch(FSM_EVENT) {
 559                 case EV_ENTRY:
 560                         robot_trajectory_new_backward(&tcFast);
 561                         robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90));
 562                         break;
 563                 case EV_MOTION_DONE:
 564                         where_to_go = CORN;
 565                         FSM_TRANSITION(rush_corns_decider);
 566                         break;
 567                 case EV_START:
 568                 case EV_TIMER:
 569                 case EV_RETURN:
 570                 case EV_VIDLE_DONE:
 571                 case EV_MOTION_ERROR:
 572                 case EV_SWITCH_STRATEGY:
 573                         DBG_PRINT_EVENT("unhandled event");
 574                 case EV_EXIT:
 575                         break;
 576         }
 577 }