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