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