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
  20 #include "common-states.h"
  21
  22 static void set_initial_position()
  23 {
  24         robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
  25                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
  26                                 DEG2RAD(180));
  27 }
  28
  29 static void actuators_home()
  30 {
  31         act_vidle(VIDLE_UP);
  32 }
  33
  34
  35
  36 void start_entry()
  37 {
  38         pthread_t thid;
  39         robot.check_turn_safety = false;
  40         pthread_create(&thid, NULL, timing_thread, NULL);
  41         start_timer();
  42 }
  43
  44 // We set initial position periodically in order for it to be updated
  45 // on the display if the team color is changed during waiting for
  46 // start.
  47 void start_timer()
  48 {
  49         set_initial_position();
  50         if (robot.start_state == START_PLUGGED_IN)
  51                 actuators_home();
  52 }
  53
  54 void start_go()
  55 {
  56         sem_post(&robot.start);
  57         actuators_home();
  58         set_initial_position();
  59 }
  60
  61 void start_exit()
  62 {
  63         robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center);
  64 }
  65
  66 /************************************************************************
  67  * Trajectory constraints used, are initialized in the init state
  68  ************************************************************************/
  69
  70 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
  71
  72
  73
  74 FSM_STATE(zvedej_vidle)
  75 {
  76         static int cnt = 0;
  77         switch(FSM_EVENT) {
  78                 case EV_ENTRY:
  79                 case EV_TIMER:
  80                         FSM_TIMER(500);
  81                         act_vidle((VIDLE_UP - VIDLE_DOWN)*cnt/3 + VIDLE_DOWN);
  82                         printf("--------------------cnt: %d\n", cnt);
  83                         cnt++;
  84                         if(cnt >= 3) {
  85                                 robot_exit();
  86                                 //FSM_TRANSITION(sledge_down);
  87                         }
  88                         break;
  89                 case EV_START:
  90                 case EV_RETURN:
  91                 case EV_ACTION_DONE:
  92                 case EV_ACTION_ERROR:
  93                 case EV_MOTION_DONE:
  94                 case EV_GOAL_NOT_REACHABLE:
  95                         DBG_PRINT_EVENT("unhandled event");
  96                 case EV_EXIT:
  97                         break;
  98         }
  99 }
 100
 101 static enum strategy {
 102         OPPONENTS_ORAGES_TOO,
 103         CORNS_ONLY
 104 } strategy = OPPONENTS_ORAGES_TOO;
 105
 106 static enum which_side which_slope;
 107 static int slope_cnt = 0;
 108 FSM_STATE(approach_the_slope)
 109 {
 110         switch(FSM_EVENT) {
 111                 case EV_ENTRY:
 112                         if (strategy == OPPONENTS_ORAGES_TOO) {
 113                                 if (slope_cnt == 0) {
 114                                         which_slope = MINE;
 115                                         FSM_TRANSITION(climb_the_slope);
 116                                 } else if (slope_cnt == 1) {
 117                                         which_slope = OPPONENTS;
 118                                         robot_goto_trans(
 119                                                 x_coord(0.3, which_slope),
 120                                                 PLAYGROUND_HEIGHT_M - ROBOT_WIDTH_M/2 - 0.03,
 121                                                 ARRIVE_FROM(DEG2RAD(0), 0.02),
 122                                                 &tcFast);
 123                                 } else {
 124                                         which_slope = MINE;
 125                                         FSM_TRANSITION(approach_next_corn);
 126                                 }
 127                         } else if (strategy == CORNS_ONLY) {
 128                                 if (slope_cnt == 0) {
 129                                         which_slope = MINE;
 130                                         FSM_TRANSITION(climb_the_slope);
 131                                 } else if (slope_cnt == 1) {
 132                                         FSM_TRANSITION(approach_next_corn);
 133                                 }
 134                         }
 135                         slope_cnt++;
 136                         break;
 137                 case EV_MOTION_DONE:
 138                         FSM_TRANSITION(climb_the_slope);
 139                         break;
 140                 case EV_START:
 141                 case EV_TIMER:
 142                 case EV_RETURN:
 143                 case EV_ACTION_DONE:
 144                 case EV_ACTION_ERROR:
 145                 case EV_GOAL_NOT_REACHABLE:
 146                         DBG_PRINT_EVENT("unhandled event");
 147                 case EV_EXIT:
 148                         break;
 149         }
 150 }
 151
 152 FSM_STATE(climb_the_slope)
 153 {
 154         switch(FSM_EVENT) {
 155                 case EV_ENTRY:
 156                         // disables using side switches on bumpers when going up
 157                         robot.use_left_switch = false;
 158                         robot.use_right_switch = false;
 159                         robot.use_back_switch = false;
 160                         act_vidle(VIDLE_DOWN);
 161                         robot.ignore_hokuyo = true;
 162                         robot_trajectory_new_backward(&tcSlow);
 163                         if (which_slope == MINE) {
 164                                 robot_trajectory_add_point_trans(
 165                                         x_coord(0.5 - ROBOT_AXIS_TO_BACK_M, which_slope),
 166                                         PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
 167                         } else {
 168                                 robot_trajectory_add_point_trans(
 169                                         x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, which_slope),
 170                                         PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01);
 171                         }
 172                         /* position for collecting oranges*/
 173                         robot_trajectory_add_final_point_trans(
 174                                 x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, which_slope),
 175                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
 176                                 NO_TURN());
 177                         break;
 178                 case EV_MOTION_DONE:
 179                         FSM_TIMER(2000);
 180                         act_vidle(VIDLE_UP);
 181                         break;
 182                 case EV_START:
 183                 case EV_TIMER:
 184                         FSM_TRANSITION(sledge_down);
 185                         break;
 186                 case EV_RETURN:
 187                 case EV_ACTION_DONE:
 188                 case EV_ACTION_ERROR:
 189                 case EV_GOAL_NOT_REACHABLE:
 190                         DBG_PRINT_EVENT("unhandled event");
 191                 case EV_EXIT:
 192                         break;
 193         }
 194 }
 195
 196 FSM_STATE(sledge_down)
 197 {
 198         switch(FSM_EVENT) {
 199                 case EV_ENTRY:
 200                         robot_trajectory_new(&tcFast);
 201                         robot_trajectory_add_point_trans(
 202                                 x_coord(1 - ROBOT_AXIS_TO_BACK_M, which_slope),
 203                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01);
 204                         robot_trajectory_add_point_trans(
 205                                 x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, which_slope),
 206                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
 207                         robot_trajectory_add_final_point_trans(
 208                                 x_coord(0.50, which_slope),
 209                                 PLAYGROUND_HEIGHT_M - 0.6,
 210                                 NO_TURN());
 211                         break;
 212                 case EV_MOTION_DONE:
 213                         act_vidle(VIDLE_UP);
 214                         FSM_TRANSITION(to_container_diag);
 215                         //FSM_TRANSITION(to_container_ortho);
 216                         break;
 217                 case EV_START:
 218                 case EV_TIMER:
 219                 case EV_RETURN:
 220                 case EV_ACTION_DONE:
 221                 case EV_ACTION_ERROR:
 222                 case EV_GOAL_NOT_REACHABLE:
 223                         DBG_PRINT_EVENT("unhandled event");
 224                 case EV_EXIT:
 225                         // enables using side switches on bumpers
 226                         robot.use_left_switch = true;
 227                         robot.use_right_switch = true;
 228                         robot.use_back_switch = true;
 229                         robot.ignore_hokuyo = false;
 230                         robot.check_turn_safety = true;
 231
 232                         break;
 233         }
 234 }
 235
 236 FSM_STATE(to_container_diag)
 237 {
 238         switch(FSM_EVENT) {
 239                 case EV_ENTRY:
 240                         if (which_slope == MINE) {
 241                                 robot_trajectory_new(&tcFast);
 242                                 // face the rim with front of the robot
 243                                 //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
 244                                 // face the rim with back of the robot
 245                                 robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M-0.6, 0.35);
 246                                 robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
 247                         } else {
 248                                 robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcFast);
 249                         }
 250                         break;
 251                 case EV_MOTION_DONE:
 252                         FSM_TIMER(6000);
 253                         act_vidle(VIDLE_VYSIP);
 254                         break;
 255                 case EV_TIMER:
 256                         act_vidle(VIDLE_UP);
 257                         FSM_TRANSITION(approach_the_slope);
 258                         //FSM_TRANSITION(approach_next_corn);
 259                         break;
 260                 case EV_START:
 261                 case EV_RETURN:
 262                 case EV_ACTION_DONE:
 263                 case EV_ACTION_ERROR:
 264                 case EV_GOAL_NOT_REACHABLE:
 265                         DBG_PRINT_EVENT("unhandled event");
 266                 case EV_EXIT:
 267                         break;
 268         }
 269 }
 270
 271 FSM_STATE(to_container_ortho)
 272 {
 273         switch(FSM_EVENT) {
 274                 case EV_ENTRY:
 275                         robot_trajectory_new(&tcFast);
 276                         robot_trajectory_add_point_trans(
 277                                 SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.15,
 278                                 PLAYGROUND_HEIGHT_M - 0.355);
 279                         robot_trajectory_add_point_trans(0.55, PLAYGROUND_HEIGHT_M - 0.65);
 280                         robot_trajectory_add_point_trans(0.90, PLAYGROUND_HEIGHT_M - 0.75);
 281                         robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.60, PLAYGROUND_HEIGHT_M - 0.7);
 282                         robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.35, PLAYGROUND_HEIGHT_M - 0.9);
 283
 284                         // face the rim with front of the robot
 285                         //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
 286                         // face the rim with back of the robot
 287                         robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.40, TURN_CCW(DEG2RAD(90)));
 288                         break;
 289                 case EV_MOTION_DONE:
 290                         act_vidle(VIDLE_VYSIP);
 291                         break;
 292                 case EV_START:
 293                 case EV_TIMER:
 294                 case EV_RETURN:
 295                 case EV_ACTION_DONE:
 296                 case EV_ACTION_ERROR:
 297                 case EV_GOAL_NOT_REACHABLE:
 298                         DBG_PRINT_EVENT("unhandled event");
 299                 case EV_EXIT:
 300                         break;
 301         }
 302 }
 303
 304 static enum where_to_go {
 305         CORN,
 306         TURN_AROUND,
 307         CONTAINER,
 308         NO_MORE_CORN
 309 } where_to_go = CORN;
 310
 311 static struct corn *corn_to_get;
 312
 313 FSM_STATE(experiment_decider)
 314 {
 315
 316         switch(FSM_EVENT) {
 317                 case EV_ENTRY:
 318                         if (where_to_go == CORN) {
 319                                 FSM_TRANSITION(approach_next_corn);
 320                         } else if (where_to_go == CONTAINER) {
 321                                 FSM_TRANSITION(rush_the_corn);
 322                         } else if (where_to_go == TURN_AROUND) {
 323                                 FSM_TRANSITION(turn_around);
 324                         } else /* NO_MORE_CORN */ {
 325                         }
 326                         break;
 327                 case EV_START:
 328                 case EV_TIMER:
 329                 case EV_RETURN:
 330                 case EV_ACTION_DONE:
 331                 case EV_ACTION_ERROR:
 332                 case EV_MOTION_DONE:
 333                 case EV_GOAL_NOT_REACHABLE:
 334                         DBG_PRINT_EVENT("unhandled event");
 335                 case EV_EXIT:
 336                         break;
 337         }
 338 }
 339
 340 static int cnt = 0;
 341 FSM_STATE(approach_next_corn)
 342 {
 343         switch(FSM_EVENT) {
 344                 case EV_ENTRY: {
 345                                 double x, y, phi;
 346                                 robot_get_est_pos(&x, &y, &phi);
 347                                 printf("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
 348                                         cnt, x, y, phi);
 349
 350                                 corn_to_get = choose_next_corn();
 351                                 if (corn_to_get) {
 352                                         Pos *p = get_corn_approach_position(corn_to_get);
 353                                         corn_to_get->was_collected = true;
 354                                         //robot_trajectory_new(&tcFast);
 355                                         //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
 356                                         robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
 357                                         delete(p);
 358                                         where_to_go = CONTAINER;
 359                                 } else {
 360                                         where_to_go = NO_MORE_CORN;
 361                                 }
 362                                 break;
 363                         }
 364                 case EV_MOTION_DONE:
 365                         cnt++;
 366                         FSM_TRANSITION(experiment_decider);
 367                         break;
 368                 case EV_START:
 369                 case EV_TIMER:
 370                 case EV_RETURN:
 371                 case EV_ACTION_DONE:
 372                 case EV_ACTION_ERROR:
 373                 case EV_GOAL_NOT_REACHABLE:
 374                         DBG_PRINT_EVENT("unhandled event");
 375                 case EV_EXIT:
 376                         break;
 377         }
 378 }
 379
 380 FSM_STATE(rush_the_corn)
 381 {
 382         switch(FSM_EVENT) {
 383                 case EV_ENTRY:
 384                         double x;
 385                         if (robot.team_color == BLUE) {
 386                                 x = corn_to_get->position.x;
 387                         } else {
 388                                 x = PLAYGROUND_WIDTH_M - corn_to_get->position.x;
 389                         }
 390                         remove_wall_around_corn(x, corn_to_get->position.y);
 391                         robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
 392                         where_to_go = TURN_AROUND;
 393                         break;
 394                 case EV_MOTION_DONE:
 395                         FSM_TRANSITION(experiment_decider);
 396                         break;
 397                 case EV_START:
 398                 case EV_TIMER:
 399                 case EV_RETURN:
 400                 case EV_ACTION_DONE:
 401                 case EV_ACTION_ERROR:
 402                 case EV_GOAL_NOT_REACHABLE:
 403                         DBG_PRINT_EVENT("unhandled event");
 404                 case EV_EXIT:
 405                         break;
 406         }
 407 }
 408
 409 // used to perform the maneuvre
 410 FSM_STATE(turn_around)
 411 {
 412         switch(FSM_EVENT) {
 413                 case EV_ENTRY:
 414                         robot_trajectory_new_backward(&tcFast);
 415                         robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90));
 416                         break;
 417                 case EV_MOTION_DONE:
 418                         where_to_go = CORN;
 419                         FSM_TRANSITION(experiment_decider);
 420                         break;
 421                 case EV_START:
 422                 case EV_TIMER:
 423                 case EV_RETURN:
 424                 case EV_ACTION_DONE:
 425                 case EV_ACTION_ERROR:
 426                 case EV_GOAL_NOT_REACHABLE:
 427                         DBG_PRINT_EVENT("unhandled event");
 428                 case EV_EXIT:
 429                         break;
 430         }
 431 }