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