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