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 "actuators.h"
  15 #include <sharp.h>
  16 #include <trgen.h>
  17 #include "match-timing.h"
  18 #include <stdbool.h>
  19 #include <ul_log.h>
  20
  21 UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
  22
  23 #include "common-states.h"
  24
  25 /************************************************************************
  26  * Functions used in and called from all the (almost identical)
  27  * "wait for start" states in particular strategies.
  28  ************************************************************************/
  29
  30 #undef DBG_FSM_STATE
  31 #define DBG_FSM_STATE(name)     do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
  32                                                                    fsm->debug_name, robot_current_time(), \
  33                                                                    name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
  34
  35
  36 void set_initial_position()
  37 {
  38         robot_set_est_pos_trans(ROBOT_START_X_M,
  39                                 ROBOT_START_Y_M,
  40                                 DEG2RAD(ROBOT_START_ANGLE_DEG));
  41 }
  42
  43 void actuators_home()
  44 {
  45 //      act_jaws(CLOSE);
  46
  47 //        bool pokus = robot.fsm.motion.state_name == "wait_and_try_again";
  48         // drive lift to home position
  49         //act_lift(0, 0, 1);
  50         // unset the homing request
  51         //act_lift(0, 0, 0);
  52 }
  53
  54 void start_entry()
  55 {
  56         pthread_t thid;
  57         robot.check_turn_safety = false;
  58         pthread_create(&thid, NULL, timing_thread, NULL);
  59         start_timer();
  60 }
  61
  62 // We set initial position periodically in order for it to be updated
  63 // on the display if the team color is changed during waiting for
  64 // start.
  65 void start_timer()
  66 {
  67         set_initial_position();
  68         if (robot.start_state == START_PLUGGED_IN)
  69                 actuators_home();
  70 }
  71
  72 void start_go()
  73 {
  74         sem_post(&robot.start);
  75         actuators_home();
  76         set_initial_position();
  77 }
  78
  79 void start_exit()
  80 {
  81
  82 }
  83
  84 bool read_sharp()
  85 {
  86         int sharp_data = robot.orte.jaws_status.act_pos.left;
  87         int sharp_dist = 0;
  88         sharp_dist = s_gp2y0a21_ir2mmLong(sharp_data);
  89         printf("sharp data: %dmm\n", sharp_dist);
  90         return (sharp_dist <= 150 ? true : false);
  91 }
  92
  93 void inline enable_bumpers(bool enabled)
  94 {
  95         robot.use_left_bumper = enabled;
  96         robot.use_right_bumper = enabled;
  97         robot.use_back_bumpers = enabled;
  98 }
  99
 100
 101
 102 /************************************************************************
 103  * Trajectory constraints used; They are  initialized in the main() function in competition.cc
 104  ************************************************************************/
 105
 106 struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
 107 double totem_x;
 108 double totem_y;
 109 bool up;
 110
 111 FSM_STATE(approach_central_buillon)
 112 {
 113         switch(FSM_EVENT) {
 114                 case EV_ENTRY:
 115                         robot_trajectory_new(&tcSlow); // new trajectory
 116                         robot_trajectory_add_point_trans(
 117                                 0.5,
 118                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
 119                         robot_trajectory_add_point_trans(
 120                                 0.64,
 121                                 1.2);
 122                         robot_trajectory_add_point_trans(
 123                                 0.64,
 124                                 0.7);
 125                         robot_trajectory_add_final_point_trans(
 126                                 1.0,
 127                                 0.45,
 128                                 ARRIVE_FROM(DEG2RAD(24),0.1));
 129 //                      robot_trajectory_add_final_point_trans(
 130 //                              1.3,
 131 //                              0.58,
 132 //                              NO_TURN());
 133                         break;
 134                 case EV_MOTION_DONE:
 135                         FSM_TIMER(2000);
 136                         break;
 137                 case EV_TIMER:
 138                         FSM_TRANSITION(catch_central_buillon);
 139                         break;
 140                 default:
 141                         break;
 142         }
 143 }
 144
 145 FSM_STATE(catch_central_buillon)
 146 {
 147         switch(FSM_EVENT) {
 148                 case EV_ENTRY:
 149                         robot_trajectory_new(&tcSlow); // new trajectory
 150                         robot_trajectory_add_final_point_trans(
 151                                 1.3,
 152                                 0.58,
 153                                 NO_TURN());
 154                         break;
 155                 case EV_MOTION_DONE:
 156                         FSM_TIMER(2000);
 157                         break;
 158                 case EV_TIMER:
 159                         FSM_TRANSITION(leave_central_buillon);
 160                         break;
 161                 default:
 162                         break;
 163         }
 164 }
 165
 166 FSM_STATE(leave_central_buillon)
 167 {
 168         switch(FSM_EVENT) {
 169                 case EV_ENTRY:
 170                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 171                         robot_trajectory_add_final_point_trans(
 172                                 0.85,
 173                                 0.38,
 174                                 NO_TURN());
 175                         break;
 176                 case EV_MOTION_DONE:
 177                         enable_bumpers(false);
 178                         SUBFSM_RET(NULL);
 179                         break;
 180                 default:
 181                         break;
 182         }
 183 }
 184
 185 FSM_STATE(push_bottle_bw)
 186 {
 187         switch(FSM_EVENT) {
 188                 case EV_ENTRY:
 189                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 190                         robot_trajectory_add_point_trans(
 191                                 0.7,
 192                                 0.3);
 193                         robot_trajectory_add_final_point_trans(
 194                                 0.64+0.08,
 195                                 ROBOT_AXIS_TO_BACK_M + 0.05,
 196                                 ARRIVE_FROM(DEG2RAD(270), 0.10));
 197                         break;
 198                 case EV_MOTION_DONE:
 199                         FSM_TRANSITION(return_home);
 200                         break;
 201                 default:
 202                         break;
 203         }
 204 }
 205
 206 FSM_STATE(return_home)
 207 {
 208         switch(FSM_EVENT) {
 209                 case EV_ENTRY:
 210                         robot_trajectory_new(&tcSlow); // new trajectory
 211                         robot_trajectory_add_point_trans(
 212                                 0.64 + 0.08,
 213                                 0.7);
 214                         robot_trajectory_add_final_point_trans(
 215                                 0.4,
 216                                 1.0,
 217                                 ARRIVE_FROM(DEG2RAD(180), 0.10));
 218                         break;
 219                 case EV_MOTION_DONE:
 220                         enable_bumpers(true);
 221                         FSM_TIMER(2000);
 222                         break;
 223                 case EV_TIMER:
 224                         SUBFSM_RET(NULL);
 225                         break;
 226                 default:
 227                         break;
 228         }
 229 }
 230
 231 FSM_STATE(leave_home)
 232 {
 233         switch(FSM_EVENT) {
 234                 case EV_ENTRY:
 235                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 236                         robot_trajectory_add_final_point_trans(
 237                                 0.6,
 238                                 1.0,
 239                                 NO_TURN());
 240                         break;
 241                 case EV_MOTION_DONE:
 242                         FSM_TRANSITION(leave_home_for_totem);
 243                         break;
 244                 default:
 245                         break;
 246         }
 247 }
 248
 249 FSM_STATE(leave_home_for_totem)
 250 {
 251         switch(FSM_EVENT) {
 252                 case EV_ENTRY:
 253                         robot_trajectory_new(&tcSlow); // new trajectory
 254                         if(up) {
 255                                 robot_trajectory_add_final_point_trans(
 256                                         0.64,
 257                                         1.3,
 258                                         NO_TURN());
 259                         }
 260                         else {
 261                                 robot_trajectory_add_final_point_trans(
 262                                         0.64,
 263                                         0.7,
 264                                         NO_TURN());
 265                         }
 266                         break;
 267                 case EV_MOTION_DONE:
 268                         if(up) FSM_TRANSITION(approach_totem_up);
 269                         else FSM_TRANSITION(approach_totem_down);
 270                         break;
 271                 default:
 272                         break;
 273         }
 274 }
 275
 276 FSM_STATE(approach_totem_down)
 277 {
 278         switch(FSM_EVENT) {
 279                 case EV_ENTRY:
 280                         robot_trajectory_new(&tcSlow); // new trajectory
 281                         robot_trajectory_add_final_point_trans(
 282                                 totem_x,
 283                                 0.48,
 284                                 TURN_CCW(DEG2RAD(90)));
 285                         break;
 286                 case EV_MOTION_DONE:
 287                         FSM_TIMER(2000);
 288                         break;
 289                 case EV_TIMER:
 290                         FSM_TRANSITION(catch_totem_buillon_down);
 291                         break;
 292                 default:
 293                         break;
 294         }
 295 }
 296
 297 FSM_STATE(catch_totem_buillon_down)
 298 {
 299         switch(FSM_EVENT) {
 300                 case EV_ENTRY:
 301                         robot_trajectory_new(&tcSlow); // new trajectory
 302                         robot_trajectory_add_final_point_trans(
 303                                 totem_x,
 304                                 totem_y - ROBOT_AXIS_TO_FRONT_M - 0.05,
 305                                 ARRIVE_FROM(DEG2RAD(90), 0.10));
 306                         break;
 307                 case EV_MOTION_DONE:
 308                         FSM_TIMER(2000);
 309                         break;
 310                 case EV_TIMER:
 311                         FSM_TRANSITION(leave_totem_down);
 312                 default:
 313                         break;
 314         }
 315 }
 316
 317 FSM_STATE(leave_totem_down)
 318 {
 319         switch(FSM_EVENT) {
 320                 case EV_ENTRY:
 321                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 322                         robot_trajectory_add_final_point_trans(
 323                                 totem_x,
 324                                 0.48,
 325                                 NO_TURN());
 326                         break;
 327                 case EV_MOTION_DONE:
 328                         FSM_TRANSITION(place_buillon_home);
 329                 default:
 330                         break;
 331         }
 332 }
 333
 334 FSM_STATE(place_buillon_home)
 335 {
 336         switch(FSM_EVENT) {
 337                 case EV_ENTRY:
 338                         robot_trajectory_new(&tcSlow); // new trajectory
 339                         if(up) {
 340                                 robot_trajectory_add_point_trans(
 341                                         0.9,
 342                                         1.52);
 343                         }
 344                         else {
 345                                 robot_trajectory_add_point_trans(
 346                                         0.9,
 347                                         0.48);
 348                                 robot_trajectory_add_point_trans(
 349                                         0.7,
 350                                         0.8);
 351                         }
 352                         robot_trajectory_add_final_point_trans(
 353                                 0.4,
 354                                 1.0,
 355                                 ARRIVE_FROM(DEG2RAD(180),0.10));
 356                         break;
 357                 case EV_MOTION_DONE:
 358                         FSM_TIMER(2000);
 359                         break;
 360                 case EV_TIMER:
 361                         SUBFSM_RET(NULL);
 362                 default:
 363                         break;
 364         }
 365 }
 366
 367 FSM_STATE(approach_totem_up)
 368 {
 369         switch(FSM_EVENT) {
 370                 case EV_ENTRY:
 371                         robot_trajectory_new(&tcSlow); // new trajectory
 372                         robot_trajectory_add_final_point_trans(
 373                                 totem_x,
 374                                 1.52,
 375                                 TURN_CW(DEG2RAD(270)));
 376                         break;
 377                 case EV_MOTION_DONE:
 378                         FSM_TIMER(2000);
 379                         break;
 380                 case EV_TIMER:
 381                         FSM_TRANSITION(catch_totem_buillon_up);
 382                         break;
 383                 default:
 384                         break;
 385         }
 386 }
 387
 388 FSM_STATE(catch_totem_buillon_up)
 389 {
 390         switch(FSM_EVENT) {
 391                 case EV_ENTRY:
 392                         robot_trajectory_new(&tcSlow); // new trajectory
 393                         robot_trajectory_add_final_point_trans(
 394                                 totem_x,
 395                                 totem_y + ROBOT_AXIS_TO_FRONT_M + 0.05,
 396                                 ARRIVE_FROM(DEG2RAD(270), 0.10));
 397                         break;
 398                 case EV_MOTION_DONE:
 399                         FSM_TIMER(2000);
 400                         break;
 401                 case EV_TIMER:
 402                         FSM_TRANSITION(leave_totem_up);
 403                         break;
 404                 default:
 405                         break;
 406         }
 407 }
 408
 409 FSM_STATE(leave_totem_up)
 410 {
 411         switch(FSM_EVENT) {
 412                 case EV_ENTRY:
 413                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 414                         robot_trajectory_add_final_point_trans(
 415                                 totem_x,
 416                                 1.52,
 417                                 NO_TURN());
 418                         break;
 419                 case EV_MOTION_DONE:
 420                         SUBFSM_RET(NULL);
 421                 default:
 422                         break;
 423         }
 424 }