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