src/robofsm/eb2008/fsmmain.cc

   1 /*
   2  * fsmmain.cc       08/04/29
   3  *
   4  * Robot's main control program (Eurobot 2008).
   5  *
   6  * Copyright: (c) 2008 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 <robodata.h>
  17 #include <robot_eb2008.h>
  18 #include <fsm.h>
  19 #include <unistd.h>
  20 #include <servos_eb2008.h>
  21 #include <math.h>
  22 #include "trgen.h"
  23 #include <movehelper_eb2008.h>
  24 #include <map.h>
  25 #include <sharp.h>
  26 #include <robomath.h>
  27
  28 /* define this macro */
  29 /* define in CFLAGS */
  30 // #define COMPETITION
  31
  32 #ifdef COMPETITION
  33 #define WAIT_FOR_START
  34 #define COMPETITION_TIME_DEFAULT        90
  35 #define TIME_TO_DEPOSITE_DEFAULT        60
  36 #else
  37 #undef WAIT_FOR_START
  38 #define COMPETITION_TIME_DEFAULT        900
  39 #define TIME_TO_DEPOSITE_DEFAULT        60
  40 #endif
  41
  42 /* competition time in seconds */
  43 #define COMPETITION_TIME        COMPETITION_TIME_DEFAULT
  44 #define TIME_TO_DEPOSITE        TIME_TO_DEPOSITE_DEFAULT
  45 /* competition time in seconds */
  46
  47 enum {
  48         LEFT = 0,
  49         RIGHT,
  50         CENTER
  51 };
  52
  53 int balls_to_collect = 0;
  54
  55 /************************************************************************
  56  * MISC FUNCTIONS
  57  ************************************************************************/
  58
  59 /**
  60  * Convert back sharps' measured values to mm.
  61  *
  62  * @ingroup     fsmmain
  63  */
  64 void get_back_sharp_mm(int *sharp)
  65 {
  66         ROBOT_LOCK(sharps);
  67         sharp[LEFT] = (int)(robot.sharps.back_left)*1000;
  68         sharp[RIGHT] = (int)(robot.sharps.back_right)*1000;
  69         ROBOT_UNLOCK(sharps);
  70 }
  71
  72 /**
  73  * Convert rear sharps' measured values to mm.
  74  *
  75  * @ingroup     fsmmain
  76  */
  77 void get_rear_sharp_mm(int *sharp)
  78 {
  79         ROBOT_LOCK(sharps);
  80         sharp[LEFT] = (int)(robot.sharps.left)*1000;
  81         sharp[RIGHT] = (int)(robot.sharps.right)*1000;
  82         ROBOT_UNLOCK(sharps);
  83 }
  84
  85 /**
  86  * Convert front sharps' measured values to mm.
  87  *
  88  * @ingroup     fsmmain
  89  */
  90 void get_front_sharp_mm(int *sharp)
  91 {
  92         ROBOT_LOCK(sharps);
  93         sharp[LEFT] = (int)(robot.sharps.front_left)*1000;
  94         sharp[RIGHT] = (int)(robot.sharps.front_right)*1000;
  95         ROBOT_UNLOCK(sharps);
  96 }
  97
  98 /**
  99  * Get values from front sharps.
 100  *
 101  * @ingroup     fsmmain
 102  */
 103 void get_front_sharp_m(double *sharp)
 104 {
 105         ROBOT_LOCK(sharps);
 106         sharp[LEFT] = robot.sharps.front_left;
 107         sharp[RIGHT] = robot.sharps.front_right;
 108         ROBOT_UNLOCK(sharps);
 109 }
 110
 111 /**
 112  * Get values from rear sharps.
 113  *
 114  * @ingroup     fsmmain
 115  */
 116 void get_rear_sharp_m(double *sharp)
 117 {
 118         ROBOT_LOCK(sharps);
 119         sharp[LEFT] = robot.sharps.left;
 120         sharp[RIGHT] = robot.sharps.right;
 121         ROBOT_UNLOCK(sharps);
 122 }
 123
 124 /**
 125  * Get values from back sharps.
 126  *
 127  * @ingroup     fsmmain
 128  */
 129 void get_back_sharp_m(double *sharp)
 130 {
 131         ROBOT_LOCK(sharps);
 132         sharp[LEFT] = robot.sharps.back_left;
 133         sharp[RIGHT] = robot.sharps.back_right;
 134         ROBOT_UNLOCK(sharps);
 135 }
 136
 137 /**
 138  * Use bumpers check if we are closed to the dispenser
 139  */
 140 int closed_to_dispenser()
 141 {
 142         int rv = 0;
 143
 144         ROBOT_LOCK(bumper);
 145         rv = robot.bumper.left | robot.bumper.right;
 146         ROBOT_UNLOCK(bumper);
 147
 148         return rv;
 149 }
 150
 151 int closed_to_container()
 152 {
 153         int rv = 0;
 154
 155         ROBOT_LOCK(sharps);
 156         rv = ((robot.sharps.back_left < 0.05)
 157                         && (robot.sharps.back_right < 0.05));
 158         ROBOT_UNLOCK(sharps);
 159
 160         /* FIXME: */
 161         return 1;
 162 }
 163
 164 int get_ball_number()
 165 {
 166         static int cycle = 0;
 167         int rv = 0;
 168
 169         cycle++;
 170
 171         switch (cycle) {
 172                 case 1:
 173                 case 4:
 174                         rv = 3;
 175                         break;
 176                 case 2:
 177                 case 3:
 178                         rv = 2;
 179                         break;
 180                 default:
 181                         rv = 1;
 182                         break;
 183         }
 184
 185         return rv;
 186 }
 187
 188 /**
 189  * Competition timer. Stop robot when the timer exceeds.
 190  *
 191  */
 192 void *wait_for_end(void *arg)
 193 {
 194         sleep(COMPETITION_TIME);
 195         printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
 196         robot_exit();
 197         return NULL;
 198 }
 199
 200 /**
 201  * Timer to go to tray.
 202  *
 203  */
 204 void *wait_to_deposition(void *arg)
 205 {
 206         sleep(TIME_TO_DEPOSITE);
 207         FSM_SIGNAL(MAIN, EV_SHORT_TIME_TO_END, NULL);
 208         return NULL;
 209 }
 210
 211 /**
 212  * Get position of the point when we know the distance and angle to turn.
 213  *
 214  * @param act   actual position
 215  * @param pos   countered position
 216  */
 217 void get_relative_pos(struct est_pos_type *est, struct ref_pos_type *ref,
 218                         double l, double phi)
 219 {
 220         ref->x = est->x + l*cos(est->phi + phi);
 221         ref->y = est->y + l*sin(est->phi + phi);
 222         ref->phi = est->phi + phi;
 223 }
 224
 225 void robot_goto_point(struct ref_pos_type des_pos)
 226 {
 227         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 228
 229         tc.maxv /= 4;
 230         robot_trajectory_new(&tc);
 231         robot_trajectory_add_final_point_trans(des_pos.x, des_pos.y, NO_TURN());
 232 }
 233
 234 void robot_go_backward_to_point(struct ref_pos_type des_pos)
 235 {
 236         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
 237
 238         tc.maxv /= 1;
 239         robot_trajectory_new_backward(&tc);
 240         robot_trajectory_add_final_point_notrans(des_pos.x, des_pos.y, NO_TURN());
 241 }
 242
 243 void trans_callback(struct robo_fsm *fsm)
 244 {
 245         if(fsm->state_name!=NULL)
 246                 strncpy(robot.act_fsm_state_name, fsm->state_name, FSM_STATE_NAME_MAX_LEN);
 247
 248 }
 249
 250 void move_trans_callback(struct robo_fsm *fsm)
 251 {
 252         if(fsm->state_name!=NULL)
 253                 strncpy(robot.move_fsm_state_name, fsm->state_name, FSM_STATE_NAME_MAX_LEN);
 254
 255 }
 256 /************************************************************************
 257  * FSM STATES DECLARATION
 258  ************************************************************************/
 259
 260 /* initial and starting states */
 261 FSM_STATE_DECL(init);
 262 FSM_STATE_DECL(wait_for_start);
 263 FSM_STATE_DECL(decide_where_now);
 264 /* movement states */
 265 FSM_STATE_DECL(go_to_our_white_dispenser);
 266 FSM_STATE_DECL(go_to_our_red_dispenser);
 267 FSM_STATE_DECL(go_to_opponent_white_dispenser);
 268 FSM_STATE_DECL(try_to_approach_dispenser);
 269 FSM_STATE_DECL(go_to_container);
 270 FSM_STATE_DECL(go_back_to_edge);
 271 /* ball and carousel states */
 272 FSM_STATE_DECL(get_balls);
 273 FSM_STATE_DECL(next_carousel_position);
 274 FSM_STATE_DECL(deposite_balls);
 275
 276 /************************************************************************
 277  * INITIAL AND STARTING STATES
 278  ************************************************************************/
 279
 280 FSM_STATE(init)
 281 {
 282         /* start event ocurred */
 283         switch (FSM_EVENT) {
 284                 case EV_ENTRY:
 285                         robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
 286                                                 PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
 287                                                 DEG2RAD(-45));
 288                         off_brush_left();
 289                         off_brush_right();
 290                         brushes_out();
 291                         open_bottom_door();
 292                         close_back_door();
 293                         FSM_TRANSITION(wait_for_start);
 294 //                      FSM_TRANSITION(go_to_container);
 295 //                      FSM_TRANSITION(get_balls);
 296                         break;
 297                 default: break;
 298         }
 299 }
 300
 301 FSM_STATE(wait_for_start)
 302 {
 303         switch (FSM_EVENT) {
 304 #ifdef WAIT_FOR_START
 305                 case EV_START: {
 306                         pthread_t thid;
 307
 308                         /* start competition timer */
 309                         pthread_create(&thid, NULL, wait_for_end, NULL);
 310                         pthread_create(&thid, NULL, wait_to_deposition, NULL);
 311                 }
 312 #else
 313                 case EV_ENTRY:
 314 #endif
 315                         robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
 316                         ORTEPublicationSend(robot.orte.publication_laser_cmd);
 317
 318                         /* start to do something */
 319                         FSM_TRANSITION(decide_where_now);
 320                         break;
 321                 default: break;
 322         }
 323 }
 324
 325 FSM_STATE(decide_where_now)
 326 {
 327         static int cycle = 0;
 328
 329         switch (FSM_EVENT) {
 330                 case EV_ENTRY:
 331                         cycle++;
 332                         off_bagr();
 333                         off_brush_left();
 334                         off_brush_right();
 335                         if (robot.carousel_cnt >= BALL_TO_COLLECT ||
 336                                 robot.carousel_cnt >= CAROUSEL_SIZE) {
 337                                 FSM_TRANSITION(go_to_container);
 338                         } else {
 339                                 switch (cycle) {
 340                                 case 1:
 341                                 case 4:
 342                                         FSM_TRANSITION(go_to_our_white_dispenser);
 343                                         break;
 344                                 case 2:
 345                                 case 5:
 346                                         FSM_TRANSITION(go_to_our_red_dispenser);
 347                                         break;
 348                                 case 3:
 349                                 case 6:
 350                                         FSM_TRANSITION(go_to_container);
 351                                         break;
 352                                 case 7:
 353                                 case 8:
 354                                         FSM_TRANSITION(go_to_opponent_white_dispenser);
 355                                         break;
 356                                 default:
 357                                         if (cycle % 2) {
 358                                                 FSM_TRANSITION(go_to_our_white_dispenser);
 359                                         } else {
 360                                                 FSM_TRANSITION(go_to_container);
 361                                         }
 362                                         break;
 363                                 }
 364                         }
 365                 default: break;
 366         }
 367 }
 368
 369 /************************************************************************
 370  * MOVEMENT STATES
 371  ************************************************************************/
 372
 373 FSM_STATE(go_to_our_white_dispenser)
 374 {
 375         switch (FSM_EVENT) {
 376                 case EV_ENTRY:
 377                         robot_trajectory_new(NULL);
 378                         robot_trajectory_add_point_trans(0.7,
 379                                         PLAYGROUND_HEIGHT_M - 0.65);
 380                         robot_trajectory_add_final_point_trans(0.25,
 381                                         PLAYGROUND_HEIGHT_M - 0.65, NO_TURN());
 382                         break;
 383                 case EV_MOTION_DONE:
 384                         SUBFSM_TRANSITION(try_to_approach_dispenser, NULL);
 385                         break;
 386                 case EV_RETURN:
 387                         balls_to_collect = get_ball_number();
 388                         FSM_TRANSITION(get_balls);
 389                         break;
 390                 default: break;
 391         }
 392 }
 393
 394 FSM_STATE(go_to_our_red_dispenser)
 395 {
 396         switch (FSM_EVENT) {
 397                 case EV_ENTRY:
 398                         robot_trajectory_new(NULL);
 399                         robot_trajectory_add_point_trans(0.7,
 400                                         PLAYGROUND_HEIGHT_M - 0.45);
 401                         robot_trajectory_add_final_point_trans(0.7,
 402                                         PLAYGROUND_HEIGHT_M - 0.25, NO_TURN());
 403                         break;
 404                 case EV_MOTION_DONE:
 405                         SUBFSM_TRANSITION(try_to_approach_dispenser, NULL);
 406                         break;
 407                 case EV_RETURN:
 408                         balls_to_collect = get_ball_number();
 409                         FSM_TRANSITION(get_balls);
 410                         break;
 411                 default: break;
 412         }
 413 }
 414
 415 FSM_STATE(go_to_opponent_white_dispenser)
 416 {
 417         switch (FSM_EVENT) {
 418                 case EV_ENTRY:
 419                         robot_trajectory_new(NULL);
 420                         robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.7,
 421                                         PLAYGROUND_HEIGHT_M - 0.65);
 422                         robot_trajectory_add_final_point_trans(
 423                                         PLAYGROUND_WIDTH_M - 0.25,
 424                                         PLAYGROUND_HEIGHT_M - 0.7, NO_TURN());
 425                         break;
 426                 case EV_MOTION_DONE:
 427                         SUBFSM_TRANSITION(try_to_approach_dispenser, NULL);
 428                         break;
 429                 case EV_RETURN:
 430                         balls_to_collect = get_ball_number();
 431                         FSM_TRANSITION(get_balls);
 432                         break;
 433                 default: break;
 434         }
 435 }
 436
 437 #define APPROACHING_ATTEMPS     1
 438
 439 FSM_STATE(try_to_approach_dispenser)
 440 {
 441         static int approaching_attempts = 0;
 442         static unsigned char backward = 1;
 443
 444         switch (FSM_EVENT) {
 445                 case EV_ENTRY:
 446                         if (closed_to_dispenser()) {
 447                                 SUBFSM_RET(NULL);
 448                                 approaching_attempts = 0;
 449                         } else {
 450                                 robot_move_by(0.1, NO_TURN(), NULL);
 451                                 backward = 0;
 452                         }
 453                         break;
 454                 case EV_MOTION_DONE:
 455                         if (closed_to_dispenser() || (approaching_attempts++ > APPROACHING_ATTEMPS))  {
 456                                 SUBFSM_RET(NULL);
 457                         } else {
 458                                 if (backward) {
 459                                         FSM_TRANSITION(try_to_approach_dispenser);
 460                                 } else {
 461                                         robot_move_by(-0.1, NO_TURN(), NULL);
 462                                         backward = 1;
 463                                 }
 464                         }
 465                         break;
 466                 default: break;
 467         }
 468 }
 469
 470 #define GO_TO_CONTAINER_TIMER   10000
 471
 472 FSM_STATE(go_to_container)
 473 {
 474         switch (FSM_EVENT) {
 475                 case EV_ENTRY:
 476                         off_brush_left();
 477                         off_brush_right();
 478                         off_bagr();
 479                         robot_trajectory_new(NULL);
 480                         robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.6, 1.0);
 481                         robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M - 0.4,
 482                                                 0.4, TURN(90));
 483                         /*FSM_TIMER(GO_TO_CONTAINER_TIMER);*/
 484                         break;
 485                 case EV_TIMER:
 486                 case EV_MOTION_DONE:
 487                         FSM_TRANSITION(go_back_to_edge);
 488                         break;
 489                 default: break;
 490         }
 491 }
 492
 493 FSM_STATE(go_back_to_edge)
 494 {
 495         switch (FSM_EVENT) {
 496                 case EV_ENTRY:
 497                         robot_move_by(-0.4, NO_TURN(), NULL);
 498                         break;
 499                 case EV_TIMER:
 500                 case EV_MOTION_DONE:
 501                         if (closed_to_container()) {
 502                                 robot.carousel_pos = 0;
 503                                 FSM_TRANSITION(deposite_balls);
 504                         } else {
 505                                 DBG("FIXME: go_closer_to_container\n");
 506                         }
 507                         break;
 508                 default: break;
 509         }
 510 }
 511
 512 /************************************************************************
 513  * BALLS AND CAROUSEL MANIPULATION STATES
 514  ************************************************************************/
 515
 516 #define MAX_GET_BALL_ATTEMPS    4
 517 #define GET_BALL_TIMER          1500
 518 #define WAIT_BALL_INSIDE        5000
 519 #define GET_BALL_BAGR_SPEED     120
 520
 521 FSM_STATE(get_balls)
 522 {
 523         static int get_ball_attemps = 0;
 524         static int ball_inside = 0;
 525         static int last_get = 0;
 526
 527         switch (FSM_EVENT) {
 528                 case EV_ENTRY:
 529                 case EV_RETURN:
 530                         if (last_get) {
 531                                 robot_move_by(-0.2, NO_TURN(), NULL);
 532                                 last_get = 0;
 533                         } else {
 534                                 get_ball_attemps = 0;
 535                                 DBG("balls_to_collect = %d\n", balls_to_collect);
 536                                 open_bottom_door();
 537                                 set_bagr(GET_BALL_BAGR_SPEED);
 538                                 brushes_drives_in();
 539                                 FSM_TIMER(500);
 540                         }
 541                         break;
 542                 case EV_TIMER:
 543                         if (ball_inside) {
 544                                 ball_inside = 0;
 545                                 close_bottom_door();
 546                                 off_bagr();
 547                                 if (robot.carousel_cnt >= balls_to_collect) {
 548                                         last_get = 1;
 549                                 }
 550                                 SUBFSM_TRANSITION(
 551                                         next_carousel_position, NULL);
 552                                 robot.carousel_pos =
 553                                         (robot.carousel_pos+2) % CAROUSEL_SIZE;
 554                         } else if (get_ball_attemps++ < MAX_GET_BALL_ATTEMPS) {
 555                                 FSM_TIMER(GET_BALL_TIMER);
 556                         } else {
 557                                 robot_move_by(-0.2, NO_TURN(), NULL);
 558                         }
 559                         break;
 560                 case EV_BALL_INSIDE:
 561                         /* ball is already inside */
 562                         if (ball_inside)
 563                                 break;
 564
 565                         ball_inside = 1;
 566                         robot.carousel[robot.carousel_pos % CAROUSEL_SIZE] =
 567                                 (enum ball_color)robot.cmu.color;
 568                         robot.carousel_cnt++;
 569                         DBG("collected balls = %d\n", robot.carousel_cnt);
 570                         FSM_TIMER(WAIT_BALL_INSIDE);
 571                         break;
 572                 case EV_MOTION_DONE:
 573                         FSM_TRANSITION(decide_where_now);
 574                         break;
 575                 default: break;
 576         }
 577 }
 578
 579 #define MAX_CAROUSEL_ATTEMPTS   3
 580 #define CAROUSEL_ATTEMPT_TIMER  1000
 581
 582 FSM_STATE(next_carousel_position)
 583 {
 584         static int carousel_attempts;
 585         int rv;
 586
 587         switch (FSM_EVENT) {
 588                 case EV_ENTRY:
 589                         carousel_attempts = 0;
 590                         DBG("carousel_pos = %d\n", robot.carousel_pos);
 591                         set_carousel(robot.carousel_pos);
 592                         FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
 593                         break;
 594                 case EV_TIMER:
 595                         carousel_attempts++;
 596                         /* check out, if the carousel reached position */
 597                         ROBOT_LOCK(drives);
 598                         rv = (robot.drives.carousel_pos == robot.carousel_pos);
 599                         ROBOT_UNLOCK(drives);
 600                         if (!rv && carousel_attempts++ < MAX_CAROUSEL_ATTEMPTS) {
 601                                 FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
 602                         } else if(rv) {
 603                                 DBG("carousel reached the position.\n");
 604                                 SUBFSM_RET(NULL);
 605                         } else {
 606                                 DBG("FIXME: carousel lost.\n");
 607                                 SUBFSM_RET(NULL);
 608                         }
 609                         break;
 610                 default: break;
 611         }
 612 }
 613
 614 #define WAIT_FOR_DEPOSITION_TIMER       1500
 615
 616 FSM_STATE(deposite_balls)
 617 {
 618         switch (FSM_EVENT) {
 619                 case EV_ENTRY:
 620                         if (robot.carousel_pos < CAROUSEL_SIZE) {
 621                                 close_back_door();
 622                                 SUBFSM_TRANSITION(next_carousel_position, NULL);
 623                         } else {
 624                                 robot.carousel_cnt = 0;
 625                                 robot.carousel_pos = 0;
 626                                 DBG("carousel_cnt = %d\n", robot.carousel_cnt);
 627                                 FSM_TRANSITION(decide_where_now);
 628                         }
 629                         break;
 630                 case EV_RETURN:
 631                         open_back_door();
 632                         FSM_TIMER(WAIT_FOR_DEPOSITION_TIMER);
 633                         break;
 634                 case EV_TIMER:
 635                         robot.carousel_pos++;
 636                         close_back_door();
 637                         FSM_TRANSITION(deposite_balls);
 638                         break;
 639                 default: break;
 640         }
 641 }
 642
 643 /* main loop */
 644 int main()
 645 {
 646         /* robot initialization */
 647         robot_init();
 648
 649         robot.carousel_cnt = 0;
 650         robot.carousel_pos = 0;
 651
 652         FSM(MAIN)->debug_states = 1;
 653
 654         robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
 655         robot.fsm[FSM_ID_MAIN].transition_callback = trans_callback;
 656         robot.fsm[FSM_ID_MOTION].transition_callback = move_trans_callback;
 657
 658         /* start threads and wait */
 659         robot_start();
 660
 661         robot_wait();
 662
 663         /* clean up */
 664         robot_destroy();
 665
 666         return 0;
 667 }