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);
  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;"); robot_exit();
  91         }
  92         return ret;
  93 }
  94
  95 static enum which_side which_slope;
  96
  97 /* assures that the robot is near the slope rim; if outside our starting area, approach the slope first */
  98 FSM_STATE(approach_the_slope)
  99 {
 100         switch(FSM_EVENT) {
 101                 case EV_ENTRY: {
 102                                 which_slope = (enum which_side) FSM_EVENT_INT;
 103                                 double x, y, phi;
 104                                 robot_get_est_pos_trans(&x, &y, &phi);
 105                                 /* decide */
 106                                 bool ready_to_climb_the_slope = is_ready_to_climb_the_slope(which_slope, x, y);
 107                                 /* if necessary, approach the slope */
 108                                 if (ready_to_climb_the_slope) {
 109                                         FSM_TRANSITION(climb_the_slope);
 110                                 } else {
 111                                         robot_goto_trans(
 112                                                 x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.05, which_slope),
 113                                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05,
 114                                                 ARRIVE_FROM(DEG2RAD(0),0.05),
 115                                                 &tcFast);
 116                                 }
 117                                 break;
 118                         }
 119                 case EV_MOTION_DONE:
 120                         FSM_TRANSITION(climb_the_slope);
 121                         break;
 122                 case EV_START:
 123                 case EV_TIMER:
 124                 case EV_RETURN:
 125                 case EV_ACTION_DONE:
 126                 case EV_ACTION_ERROR:
 127                 case EV_GOAL_NOT_REACHABLE:
 128                         DBG_PRINT_EVENT("unhandled event");
 129                 case EV_EXIT:
 130                         break;
 131         }
 132 }
 133
 134 void inline enable_switches(bool enabled)
 135 {
 136         robot.use_left_switch = enabled;
 137         robot.use_right_switch = enabled;
 138         robot.use_back_switch = enabled;
 139 }
 140
 141 FSM_STATE(climb_the_slope)
 142 {
 143         switch(FSM_EVENT) {
 144                 case EV_ENTRY: {
 145                                 // disables using side switches on bumpers when going up
 146                                 enable_switches(false);
 147                                 act_vidle(VIDLE_DOWN);
 148                                 robot.ignore_hokuyo = true;
 149                                 /* create the trajectory and go */
 150                                 robot_trajectory_new_backward(&tcSlow);
 151                                 robot_trajectory_add_point_trans(
 152                                         x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, which_slope),
 153                                         PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01);
 154                                 robot_trajectory_add_final_point_trans(
 155                                         x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, which_slope),
 156                                         PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
 157                                         NO_TURN());
 158                                 break;
 159                         }
 160                 case EV_MOTION_DONE:
 161                         FSM_TIMER(VIDLE_TIMEOUT);
 162                         act_vidle(VIDLE_UP);
 163                         break;
 164                 case EV_START:
 165                 case EV_TIMER:
 166                         // FIXME: respond to VIDLE EVENT
 167                         FSM_TRANSITION(sledge_down);
 168                         break;
 169                 case EV_RETURN:
 170                 case EV_ACTION_DONE:
 171                 case EV_ACTION_ERROR:
 172                 case EV_GOAL_NOT_REACHABLE:
 173                         DBG_PRINT_EVENT("unhandled event");
 174                 case EV_EXIT:
 175                         break;
 176         }
 177 }
 178
 179 FSM_STATE(sledge_down)
 180 {
 181         switch(FSM_EVENT) {
 182                 case EV_ENTRY:
 183                         robot_trajectory_new(&tcFast);
 184                         robot_trajectory_add_point_trans(
 185                                 x_coord(1 - ROBOT_AXIS_TO_BACK_M, which_slope),
 186                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01);
 187                         /*
 188                         robot_trajectory_add_point_trans(
 189                                 x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, which_slope),
 190                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
 191                         robot_trajectory_add_final_point_trans(
 192                                 x_coord(0.50, which_slope),
 193                                 PLAYGROUND_HEIGHT_M - 0.6,
 194                                 NO_TURN());
 195                         */
 196                         robot_trajectory_add_point_trans(
 197                                 x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.20, which_slope),
 198                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.08);
 199                         robot_trajectory_add_final_point_trans(
 200                                 x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, which_slope),
 201                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.14,
 202                                 NO_TURN());
 203                         break;
 204                 case EV_MOTION_DONE:
 205                         /* just for sure, try to close it one more time */
 206                         act_vidle(VIDLE_UP);
 207                         SUBFSM_RET(NULL);
 208                         break;
 209                 case EV_START:
 210                 case EV_TIMER:
 211                 case EV_RETURN:
 212                 case EV_ACTION_DONE:
 213                 case EV_ACTION_ERROR:
 214                 case EV_GOAL_NOT_REACHABLE:
 215                         DBG_PRINT_EVENT("unhandled event");
 216                 case EV_EXIT:
 217                         // enables using side switches on bumpers
 218                         enable_switches(true);
 219                         robot.ignore_hokuyo = false;
 220                         robot.check_turn_safety = true;
 221
 222                         break;
 223         }
 224 }
 225
 226 /************************************************************************
 227  * The "unload our oranges" subautomaton
 228  ************************************************************************/
 229
 230 FSM_STATE(to_cntainer_and_unld)
 231 {
 232         switch(FSM_EVENT) {
 233                 case EV_ENTRY:
 234                         /*
 235                         if (which_slope == MINE) {
 236                                 robot_trajectory_new(&tcFast);
 237                                 // face the rim with front of the robot
 238                                 //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
 239                                 // face the rim with back of the robot
 240                                 robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M-0.6, 0.35);
 241                                 robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
 242                         } else { */
 243                                 robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcFast);
 244                         //}
 245                         break;
 246                 case EV_MOTION_DONE:
 247                         FSM_TIMER(3000); // FIXME: test this
 248                         act_vidle(VIDLE_VYSIP);
 249                         break;
 250                 case EV_TIMER:
 251                         act_vidle(VIDLE_UP);
 252                         SUBFSM_RET(NULL);
 253                         break;
 254                 case EV_START:
 255                 case EV_RETURN:
 256                 case EV_ACTION_DONE:
 257                 case EV_ACTION_ERROR:
 258                 case EV_GOAL_NOT_REACHABLE:
 259                         DBG_PRINT_EVENT("unhandled event");
 260                 case EV_EXIT:
 261                         break;
 262         }
 263 }
 264
 265 /************************************************************************
 266  * The "collect corns" subautomaton
 267  ************************************************************************/
 268
 269 static enum where_to_go {
 270         CORN,
 271         TURN_AROUND,
 272         CONTAINER,
 273         NO_MORE_CORN
 274 } where_to_go = CORN;
 275
 276 static struct corn *corn_to_get;
 277
 278 FSM_STATE(rush_corns_decider)
 279 {
 280         switch(FSM_EVENT) {
 281                 case EV_ENTRY:
 282                         if (where_to_go == CORN) {
 283                                 FSM_TRANSITION(approach_next_corn);
 284                         } else if (where_to_go == CONTAINER) {
 285                                 FSM_TRANSITION(rush_the_corn);
 286                         } else if (where_to_go == TURN_AROUND) {
 287                                 FSM_TRANSITION(turn_around);
 288                         } else /* NO_MORE_CORN */ {
 289                         }
 290                         break;
 291                 case EV_START:
 292                 case EV_TIMER:
 293                 case EV_RETURN:
 294                 case EV_ACTION_DONE:
 295                 case EV_ACTION_ERROR:
 296                 case EV_MOTION_DONE:
 297                 case EV_GOAL_NOT_REACHABLE:
 298                         DBG_PRINT_EVENT("unhandled event");
 299                 case EV_EXIT:
 300                         break;
 301         }
 302 }
 303
 304 static int cnt = 0;
 305 FSM_STATE(approach_next_corn)
 306 {
 307         switch(FSM_EVENT) {
 308                 case EV_ENTRY: {
 309                                 double x, y, phi;
 310                                 robot_get_est_pos(&x, &y, &phi);
 311                                 printf("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
 312                                         cnt, x, y, phi);
 313
 314                                 corn_to_get = choose_next_corn();
 315                                 if (corn_to_get) {
 316                                         Pos *p = get_corn_approach_position(corn_to_get);
 317                                         corn_to_get->was_collected = true;
 318                                         //robot_trajectory_new(&tcFast);
 319                                         //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
 320                                         robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
 321                                         delete(p);
 322                                         where_to_go = CONTAINER;
 323                                 } else {
 324                                         where_to_go = NO_MORE_CORN;
 325                                 }
 326                                 break;
 327                         }
 328                 case EV_MOTION_DONE:
 329                         cnt++;
 330                         FSM_TRANSITION(rush_corns_decider);
 331                         break;
 332                 case EV_START:
 333                 case EV_TIMER:
 334                 case EV_RETURN:
 335                 case EV_ACTION_DONE:
 336                 case EV_ACTION_ERROR:
 337                 case EV_GOAL_NOT_REACHABLE:
 338                         DBG_PRINT_EVENT("unhandled event");
 339                 case EV_EXIT:
 340                         break;
 341         }
 342 }
 343
 344 FSM_STATE(rush_the_corn)
 345 {
 346         switch(FSM_EVENT) {
 347                 case EV_ENTRY:
 348                         double x;
 349                         if (robot.team_color == BLUE) {
 350                                 x = corn_to_get->position.x;
 351                         } else {
 352                                 x = PLAYGROUND_WIDTH_M - corn_to_get->position.x;
 353                         }
 354                         remove_wall_around_corn(x, corn_to_get->position.y);
 355                         robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
 356                         where_to_go = TURN_AROUND;
 357                         break;
 358                 case EV_MOTION_DONE:
 359                         FSM_TRANSITION(rush_corns_decider);
 360                         break;
 361                 case EV_START:
 362                 case EV_TIMER:
 363                 case EV_RETURN:
 364                 case EV_ACTION_DONE:
 365                 case EV_ACTION_ERROR:
 366                 case EV_GOAL_NOT_REACHABLE:
 367                         DBG_PRINT_EVENT("unhandled event");
 368                 case EV_EXIT:
 369                         break;
 370         }
 371 }
 372
 373 // used to perform the maneuvre
 374 FSM_STATE(turn_around)
 375 {
 376         switch(FSM_EVENT) {
 377                 case EV_ENTRY:
 378                         robot_trajectory_new_backward(&tcFast);
 379                         robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90));
 380                         break;
 381                 case EV_MOTION_DONE:
 382                         where_to_go = CORN;
 383                         FSM_TRANSITION(rush_corns_decider);
 384                         break;
 385                 case EV_START:
 386                 case EV_TIMER:
 387                 case EV_RETURN:
 388                 case EV_ACTION_DONE:
 389                 case EV_ACTION_ERROR:
 390                 case EV_GOAL_NOT_REACHABLE:
 391                         DBG_PRINT_EVENT("unhandled event");
 392                 case EV_EXIT:
 393                         break;
 394         }
 395 }