src/robofsm/homologation.cc

   1 /*
   2  * homologation.cc       08/04/29
   3  *
   4  * Robot's control program intended for homologation (approval phase) on Eurobot 2009.
   5  *
   6  * Copyright: (c) 2009 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 #ifndef DEBUG
  12 #define DEBUG
  13 #endif
  14
  15 #define FSM_MAIN
  16 #include <robot.h>
  17 #include <fsm.h>
  18 #include <unistd.h>
  19 #include <math.h>
  20 #include <movehelper.h>
  21 #include <map.h>
  22 #include <sharp.h>
  23 #include <robomath.h>
  24 #include <string.h>
  25 #include <robodim.h>
  26 #include <error.h>
  27 #include "actuators.h"
  28 #include <trgen.h>
  29 #include "match-timing.h"
  30 #include "eb2010misc.h"
  31 #include <ul_log.h>
  32
  33 UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file */
  34
  35 /************************************************************************
  36  * Trajectory constraints used, are initialized in the init state
  37  ************************************************************************/
  38
  39 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
  40
  41 /************************************************************************
  42  * FSM STATES DECLARATION
  43  ************************************************************************/
  44
  45 /* initial and starting states */
  46 FSM_STATE_DECL(init);
  47 FSM_STATE_DECL(wait_for_start);
  48 /* movement states */
  49 FSM_STATE_DECL(aproach_first_fugure);
  50 FSM_STATE_DECL(load_first_figure);
  51 FSM_STATE_DECL(to_red_square);
  52 FSM_STATE_DECL(go_home);
  53 FSM_STATE_DECL(go_out);
  54 // FSM_STATE_DECL(experiment_decider);
  55 // FSM_STATE_DECL(approach_next_corn);
  56 // FSM_STATE_DECL(rush_the_corn);
  57 // FSM_STATE_DECL(turn_around);
  58 // FSM_STATE_DECL(zvedej_vidle);
  59
  60 /************************************************************************
  61  * INITIAL AND STARTING STATES
  62  ************************************************************************/
  63
  64 FSM_STATE(init)
  65 {
  66         switch (FSM_EVENT) {
  67                 case EV_ENTRY:
  68                         tcSlow = trajectoryConstraintsDefault;
  69                         tcSlow.maxv = 0.3;
  70                         tcSlow.maxacc = 0.3;
  71                         tcSlow.maxomega = 1;
  72                         FSM_TRANSITION(wait_for_start);
  73                         break;
  74                 default:
  75                         break;
  76         }
  77 }
  78
  79 void set_initial_position()
  80 {
  81         //FIXME:
  82         robot_set_est_pos_trans(0.4 - ROBOT_AXIS_TO_FRONT_M,
  83                                 PLAYGROUND_HEIGHT_M - 0.21,
  84                                 0);
  85 }
  86
  87 void actuators_home()
  88 {
  89         act_jaws(CLOSE);
  90         // drive lift to home position
  91         act_lift(0, 0, 1);
  92         // unset the homing request
  93         act_lift(0, 0, 0);
  94 }
  95
  96 #ifdef COMPETITION
  97 #define WAIT_FOR_START
  98 #else
  99 #undef WAIT_FOR_START
 100 #endif
 101
 102 FSM_STATE(wait_for_start)
 103 {
 104         pthread_t thid;
 105         #ifdef WAIT_FOR_START
 106                 ul_logdeb("WAIT_FOR_START mode set\n");
 107         #else
 108                 ul_logdeb("WAIT_FOR_START mode NOT set\n");
 109         #endif
 110         #ifdef COMPETITION
 111                 ul_logdeb("COMPETITION mode set\n");
 112         #else
 113                 ul_logdeb("COMPETITION mode NOT set\n");
 114         #endif
 115         switch (FSM_EVENT) {
 116                 case EV_ENTRY:
 117                         pthread_create(&thid, NULL, timing_thread, NULL);
 118 #ifdef WAIT_FOR_START
 119                         FSM_TIMER(1000);
 120                         break;
 121 #endif
 122                 case EV_START:
 123                         /* start competition timer */
 124                         sem_post(&robot.start);
 125                         actuators_home();
 126                         set_initial_position();
 127                         FSM_TRANSITION(aproach_first_fugure);
 128                         break;
 129                 case EV_TIMER:
 130                         FSM_TIMER(1000);
 131                         // We set initial position periodically in
 132                         // order for it to be updated on the display
 133                         // if the team color is changed during waiting
 134                         // for start.
 135                         set_initial_position();
 136                         if (robot.start_state == START_PLUGGED_IN)
 137                                 actuators_home();
 138                         break;
 139                 case EV_RETURN:
 140                 case EV_MOTION_ERROR:
 141                 case EV_MOTION_DONE:
 142                 //case EV_VIDLE_DONE:
 143                 case EV_SWITCH_STRATEGY:
 144                         DBG_PRINT_EVENT("unhandled event");
 145                         break;
 146                 case EV_EXIT:
 147                         break;
 148         }
 149 }
 150
 151 // FSM_STATE(zvedej_vidle)
 152 // {
 153 //      static int cnt = 0;
 154 //      switch(FSM_EVENT) {
 155 //              case EV_ENTRY:
 156 //              case EV_TIMER:
 157 //                      FSM_TIMER(500);
 158 //                      act_vidle((VIDLE_UP - VIDLE_DOWN)*cnt/3 + VIDLE_DOWN, VIDLE_FAST_SPEED);
 159 //                      ul_logdeb("--------------------cnt: %d\n", cnt);
 160 //                      cnt++;
 161 //                      if(cnt >= 3) {
 162 //                              robot_exit();
 163 //                              //FSM_TRANSITION(sledge_down);
 164 //                      }
 165 //                      break;
 166 //              case EV_START:
 167 //              case EV_RETURN:
 168 //              case EV_VIDLE_DONE:
 169 //              case EV_MOTION_DONE:
 170 //              case EV_MOTION_ERROR:
 171 //              case EV_SWITCH_STRATEGY:
 172 //                      DBG_PRINT_EVENT("unhandled event");
 173 //              case EV_EXIT:
 174 //                      break;
 175 //      }
 176 // }
 177
 178 /************************************************************************
 179  * MOVEMENT STATES
 180  ************************************************************************/
 181
 182 FSM_STATE(aproach_first_fugure)
 183 {
 184         switch(FSM_EVENT) {
 185                 case EV_ENTRY:
 186                         // disables using side switches on bumpers when going up
 187                         robot.use_left_bumper = false;
 188                         robot.use_right_bumper = false;
 189                         robot.use_back_bumpers = false;
 190
 191                         robot_trajectory_new(&tcSlow);
 192                         //robot_move_by(0.5, NO_TURN(), &tcSlow);
 193                         robot_trajectory_add_point_trans(
 194                                 0.4+0.05+0.25,
 195                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05);
 196                         robot_trajectory_add_final_point_trans(
 197                                 0.4+0.05+0.25,
 198                                 0.29+0.28+0.28,
 199                                 TURN_CCW(0));
 200                         break;
 201                 case EV_MOTION_DONE:
 202                         FSM_TIMER(2000);
 203                         act_jaws(OPEN);
 204                         //FSM_TRANSITION(load_first_figure);
 205                         break;
 206                 case EV_START:
 207                 case EV_TIMER:
 208                         FSM_TRANSITION(load_first_figure);
 209                         break;
 210                 case EV_RETURN:
 211                 //case EV_VI_DONE:
 212                 case EV_MOTION_ERROR:
 213                 case EV_SWITCH_STRATEGY:
 214                         DBG_PRINT_EVENT("unhandled event");
 215                 case EV_EXIT:
 216                         break;
 217         }
 218 }
 219
 220 FSM_STATE(load_first_figure)
 221 {
 222         switch(FSM_EVENT) {
 223                 case EV_ENTRY:
 224                         robot_trajectory_new_backward(&tcSlow);
 225                         robot_trajectory_add_final_point_trans(
 226                                 0.2+0.1+ROBOT_AXIS_TO_BACK_M,
 227                                 0.29+0.28+0.28, NO_TURN());
 228                         break;
 229                 case EV_MOTION_DONE:
 230                         FSM_TIMER(2000);
 231                         act_jaws(CATCH);
 232                         break;
 233                 case EV_START:
 234                 case EV_TIMER:
 235                         FSM_TRANSITION(to_red_square);
 236                         break;
 237                 case EV_RETURN:
 238                 //case EV_VIDLE_DONE:
 239                 case EV_MOTION_ERROR:
 240                 case EV_SWITCH_STRATEGY:
 241                         DBG_PRINT_EVENT("unhandled event");
 242                 case EV_EXIT:
 243                         // enables using side switches on bumpers
 244                         //robot.use_left_switch = true;
 245                         //robot.use_right_switch = true;
 246                         //robot.ignore_hokuyo = false;
 247                         break;
 248         }
 249 }
 250
 251 FSM_STATE(to_red_square)
 252 {
 253         switch(FSM_EVENT) {
 254                 case EV_ENTRY:
 255                         robot.use_left_bumper = true;
 256                         robot.use_right_bumper = true;
 257                         robot.use_back_bumpers = true;
 258                         robot_trajectory_new(&tcSlow);
 259                         // face the rim with front of the robot
 260                         //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
 261                         // face the rim with back of the robot
 262                         //robot_trajectory_add_point_trans(0.4+0.05+0.2, 0.35+0.35+0.17);
 263                         robot_trajectory_add_final_point_trans(0.4+0.05+0.2 + ROBOT_AXIS_TO_FIGURE_CENTER_M + 0.05, 0.35+0.35+0.17, NO_TURN());
 264                         break;
 265                 case EV_MOTION_DONE:
 266                         FSM_TIMER(2000);
 267                         act_jaws(OPEN);
 268                         break;
 269                 case EV_TIMER:
 270                         //act_jaws(OPEN);
 271                         FSM_TRANSITION(go_out);
 272                         break;
 273                 case EV_START:
 274                 case EV_RETURN:
 275                 //case EV_VIDLE_DONE:
 276                 case EV_MOTION_ERROR:
 277                 case EV_SWITCH_STRATEGY:
 278                         DBG_PRINT_EVENT("unhandled event");
 279                 case EV_EXIT:
 280                         break;
 281         }
 282 }
 283
 284 // static enum where_to_go {
 285 //      CORN,
 286 //      TURN_AROUND,
 287 //      CONTAINER,
 288 //      NO_MORE_CORN
 289 // } where_to_go = CORN;
 290 //
 291 // static struct corn *corn_to_get;
 292 //
 293 FSM_STATE(go_out)
 294 {
 295         switch(FSM_EVENT) {
 296                 case EV_ENTRY:
 297                         robot_move_by(0.2, NO_TURN(), &tcSlow);
 298                         // face the rim with front of the robot
 299                         break;
 300                 case EV_START:
 301                 case EV_TIMER:
 302                         FSM_TRANSITION(go_home);
 303                         break;
 304                 case EV_RETURN:
 305                 case EV_MOTION_DONE:
 306                         FSM_TIMER(2000);
 307                         act_jaws(CLOSE);
 308                         break;
 309                 case EV_MOTION_ERROR:
 310                 case EV_SWITCH_STRATEGY:
 311                         DBG_PRINT_EVENT("unhandled event");
 312                 case EV_EXIT:
 313                         break;
 314         }
 315 }
 316
 317
 318 FSM_STATE(go_home)
 319 {
 320         switch(FSM_EVENT) {
 321                 case EV_ENTRY:
 322                         robot_trajectory_new(&tcSlow);
 323                         robot_trajectory_add_final_point_trans(0.4, PLAYGROUND_HEIGHT_M - 0.2, ARRIVE_FROM(DEG2RAD(180), 0.20));
 324                         break;
 325                 case EV_START:
 326                 case EV_TIMER:
 327                 case EV_RETURN:
 328                 case EV_MOTION_DONE:
 329                 case EV_MOTION_ERROR:
 330                 case EV_SWITCH_STRATEGY:
 331                         DBG_PRINT_EVENT("unhandled event");
 332                 case EV_EXIT:
 333                         break;
 334         }
 335 }
 336 // static int cnt = 0;
 337 // FSM_STATE(approach_next_corn)
 338 // {
 339 //      switch(FSM_EVENT) {
 340 //              case EV_ENTRY: {
 341 //                              double x, y, phi;
 342 //                              robot_get_est_pos(&x, &y, &phi);
 343 //                              ul_logdeb("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
 344 //                                      cnt, x, y, phi);
 345 //
 346 //                              corn_to_get = choose_next_corn();
 347 //                              if (corn_to_get) {
 348 //                                      Pos *p = get_corn_approach_position(corn_to_get);
 349 //                                      corn_to_get->was_collected = true;
 350 //                                      //robot_trajectory_new(&tcFast);
 351 //                                      //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
 352 //                                      robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
 353 //                                      delete(p);
 354 //                                      where_to_go = CONTAINER;
 355 //                              } else {
 356 //                                      where_to_go = NO_MORE_CORN;
 357 //                              }
 358 //                              break;
 359 //                      }
 360 //              case EV_MOTION_DONE:
 361 //                      cnt++;
 362 //                      FSM_TRANSITION(experiment_decider);
 363 //                      break;
 364 //              case EV_START:
 365 //              case EV_TIMER:
 366 //              case EV_RETURN:
 367 //              case EV_VIDLE_DONE:
 368 //              case EV_MOTION_ERROR:
 369 //              case EV_SWITCH_STRATEGY:
 370 //                      DBG_PRINT_EVENT("unhandled event");
 371 //              case EV_EXIT:
 372 //                      break;
 373 //      }
 374 // }
 375
 376 // FSM_STATE(rush_the_corn)
 377 // {
 378 //      switch(FSM_EVENT) {
 379 //              case EV_ENTRY:
 380 //                      double x;
 381 //                      if (robot.team_color == BLUE) {
 382 //                              x = corn_to_get->position.x;
 383 //                      } else {
 384 //                              x = PLAYGROUND_WIDTH_M - corn_to_get->position.x;
 385 //                      }
 386 //                      ul_logdeb("x = %.3f, %.3f \n", x, corn_to_get->position.y);
 387 //                      remove_wall_around_corn(x, corn_to_get->position.y);
 388 //                      robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
 389 //                      where_to_go = TURN_AROUND;
 390 //                      break;
 391 //              case EV_MOTION_DONE:
 392 //                      FSM_TRANSITION(experiment_decider);
 393 //                      break;
 394 //              case EV_START:
 395 //              case EV_TIMER:
 396 //              case EV_RETURN:
 397 //              case EV_VIDLE_DONE:
 398 //              case EV_MOTION_ERROR:
 399 //              case EV_SWITCH_STRATEGY:
 400 //                      DBG_PRINT_EVENT("unhandled event");
 401 //              case EV_EXIT:
 402 //                      break;
 403 //      }
 404 // }
 405
 406 // // used to perform the maneuvre
 407 // FSM_STATE(turn_around)
 408 // {
 409 //      switch(FSM_EVENT) {
 410 //              case EV_ENTRY:
 411 //                      robot_trajectory_new_backward(&tcFast);
 412 //                      robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90));
 413 //                      break;
 414 //              case EV_MOTION_DONE:
 415 //                      where_to_go = CORN;
 416 //                      FSM_TRANSITION(experiment_decider);
 417 //                      break;
 418 //              case EV_START:
 419 //              case EV_TIMER:
 420 //              case EV_RETURN:
 421 //              case EV_VIDLE_DONE:
 422 //              case EV_MOTION_ERROR:
 423 //              case EV_SWITCH_STRATEGY:
 424 //                      DBG_PRINT_EVENT("unhandled event");
 425 //              case EV_EXIT:
 426 //                      break;
 427 //      }
 428 // }
 429
 430
 431 /************************************************************************
 432  * MAIN FUNCTION
 433  ************************************************************************/
 434
 435 int main()
 436 {
 437         int rv;
 438
 439         rv = robot_init();
 440         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 441
 442         robot.obstacle_avoidance_enabled = true;
 443
 444         robot.fsm.main.debug_states = 1;
 445         robot.fsm.motion.debug_states = 1;
 446         //robot.fsm.act.debug_states = 1;
 447
 448         robot.fsm.main.state = &fsm_state_main_init;
 449         //robot.fsm.main.transition_callback = trans_callback;
 450         //robot.fsm.motion.transition_callback = move_trans_callback;
 451
 452         rv = robot_start();
 453         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 454
 455         robot_destroy();
 456
 457         return 0;
 458 }
 459
 460 /************************************************************************
 461  * STATE SKELETON
 462  ************************************************************************/
 463
 464 /*
 465 FSM_STATE()
 466 {
 467         switch(FSM_EVENT) {
 468                 case EV_ENTRY:
 469                         break;
 470                 case EV_START:
 471                 case EV_TIMER:
 472                 case EV_RETURN:
 473                 case EV_VIDLE_DONE:
 474                 case EV_ACTION_ERROR:
 475                 case EV_MOTION_DONE:
 476                 case EV_MOTION_ERROR:
 477                 case EV_SWITCH_STRATEGY:
 478                         DBG_PRINT_EVENT("unhandled event");
 479                 case EV_EXIT:
 480                         break;
 481         }
 482 }
 483 */