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_TIMER(2000);
 294                         break;
 295                 case EV_START_LASER:
 296                         robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
 297                                                 PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
 298                                                 DEG2RAD(-45));
 299                         robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
 300                         ORTEPublicationSend(robot.orte.publication_laser_cmd);
 301                         robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
 302                         ORTEPublicationSend(robot.orte.publication_laser_cmd);
 303                         FSM_TRANSITION(wait_for_start);
 304                         break;
 305                 case EV_TIMER:
 306 //                      FSM_TRANSITION(go_to_container);
 307 //                      FSM_TRANSITION(get_balls);
 308                         break;
 309                 default: break;
 310         }
 311 }
 312
 313 FSM_STATE(wait_for_start)
 314 {
 315         switch (FSM_EVENT) {
 316 #ifdef WAIT_FOR_START
 317                 case EV_START: {
 318                         pthread_t thid;
 319
 320                         /* start competition timer */
 321                         pthread_create(&thid, NULL, wait_for_end, NULL);
 322                         pthread_create(&thid, NULL, wait_to_deposition, NULL);
 323                 }
 324 #else
 325                 case EV_ENTRY:
 326 #endif
 327                         robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
 328                         ORTEPublicationSend(robot.orte.publication_laser_cmd);
 329
 330                         /* start to do something */
 331                         FSM_TRANSITION(decide_where_now);
 332                         break;
 333                 default: break;
 334         }
 335 }
 336
 337 FSM_STATE(decide_where_now)
 338 {
 339         static int cycle = 0;
 340
 341         switch (FSM_EVENT) {
 342                 case EV_ENTRY:
 343                         cycle++;
 344                         off_bagr();
 345                         off_brush_left();
 346                         off_brush_right();
 347                         if (robot.carousel_cnt >= BALL_TO_COLLECT ||
 348                                 robot.carousel_cnt >= CAROUSEL_SIZE) {
 349                                 FSM_TRANSITION(go_to_container);
 350                         } else {
 351                                 switch (cycle) {
 352                                 case 1:
 353                                 case 4:
 354                                         FSM_TRANSITION(go_to_our_white_dispenser);
 355                                         break;
 356                                 case 2:
 357                                 case 5:
 358                                         FSM_TRANSITION(go_to_our_red_dispenser);
 359                                         break;
 360                                 case 3:
 361                                 case 6:
 362                                         FSM_TRANSITION(go_to_container);
 363                                         break;
 364                                 case 7:
 365                                 case 8:
 366                                         FSM_TRANSITION(go_to_opponent_white_dispenser);
 367                                         break;
 368                                 default:
 369                                         if (cycle % 2) {
 370                                                 FSM_TRANSITION(go_to_our_white_dispenser);
 371                                         } else {
 372                                                 FSM_TRANSITION(go_to_container);
 373                                         }
 374                                         break;
 375                                 }
 376                         }
 377                 default: break;
 378         }
 379 }
 380
 381 /************************************************************************
 382  * MOVEMENT STATES
 383  ************************************************************************/
 384
 385 FSM_STATE(go_to_our_white_dispenser)
 386 {
 387         switch (FSM_EVENT) {
 388                 case EV_ENTRY:
 389                         robot_trajectory_new(NULL);
 390                         robot_trajectory_add_point_trans(0.7,
 391                                         PLAYGROUND_HEIGHT_M - 0.65);
 392                         robot_trajectory_add_final_point_trans(0.25,
 393                                         PLAYGROUND_HEIGHT_M - 0.65, NO_TURN());
 394                         break;
 395                 case EV_MOTION_DONE:
 396                         SUBFSM_TRANSITION(try_to_approach_dispenser, NULL);
 397                         break;
 398                 case EV_RETURN:
 399                         balls_to_collect = get_ball_number();
 400                         FSM_TRANSITION(get_balls);
 401                         break;
 402                 default: break;
 403         }
 404 }
 405
 406 FSM_STATE(go_to_our_red_dispenser)
 407 {
 408         switch (FSM_EVENT) {
 409                 case EV_ENTRY:
 410                         robot_trajectory_new(NULL);
 411                         robot_trajectory_add_point_trans(0.7,
 412                                         PLAYGROUND_HEIGHT_M - 0.45);
 413                         robot_trajectory_add_final_point_trans(0.7,
 414                                         PLAYGROUND_HEIGHT_M - 0.25, NO_TURN());
 415                         break;
 416                 case EV_MOTION_DONE:
 417                         SUBFSM_TRANSITION(try_to_approach_dispenser, NULL);
 418                         break;
 419                 case EV_RETURN:
 420                         balls_to_collect = get_ball_number();
 421                         FSM_TRANSITION(get_balls);
 422                         break;
 423                 default: break;
 424         }
 425 }
 426
 427 FSM_STATE(go_to_opponent_white_dispenser)
 428 {
 429         switch (FSM_EVENT) {
 430                 case EV_ENTRY:
 431                         robot_trajectory_new(NULL);
 432                         robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.7,
 433                                         PLAYGROUND_HEIGHT_M - 0.65);
 434                         robot_trajectory_add_final_point_trans(
 435                                         PLAYGROUND_WIDTH_M - 0.25,
 436                                         PLAYGROUND_HEIGHT_M - 0.7, NO_TURN());
 437                         break;
 438                 case EV_MOTION_DONE:
 439                         SUBFSM_TRANSITION(try_to_approach_dispenser, NULL);
 440                         break;
 441                 case EV_RETURN:
 442                         balls_to_collect = get_ball_number();
 443                         FSM_TRANSITION(get_balls);
 444                         break;
 445                 default: break;
 446         }
 447 }
 448
 449 #define APPROACHING_ATTEMPS     1
 450
 451 FSM_STATE(try_to_approach_dispenser)
 452 {
 453         static int approaching_attempts = 0;
 454         static unsigned char backward = 1;
 455
 456         switch (FSM_EVENT) {
 457                 case EV_ENTRY:
 458                         if (closed_to_dispenser()) {
 459                                 SUBFSM_RET(NULL);
 460                                 approaching_attempts = 0;
 461                         } else {
 462                                 robot_move_by(0.1, NO_TURN(), NULL);
 463                                 backward = 0;
 464                         }
 465                         break;
 466                 case EV_MOTION_DONE:
 467                         if (closed_to_dispenser() || (approaching_attempts++ > APPROACHING_ATTEMPS))  {
 468                                 SUBFSM_RET(NULL);
 469                         } else {
 470                                 if (backward) {
 471                                         FSM_TRANSITION(try_to_approach_dispenser);
 472                                 } else {
 473                                         robot_move_by(-0.1, NO_TURN(), NULL);
 474                                         backward = 1;
 475                                 }
 476                         }
 477                         break;
 478                 default: break;
 479         }
 480 }
 481
 482 #define GO_TO_CONTAINER_TIMER   10000
 483
 484 FSM_STATE(go_to_container)
 485 {
 486         switch (FSM_EVENT) {
 487                 case EV_ENTRY:
 488                         robot.obstacle_avoidance_enabled = true;
 489                         off_brush_left();
 490                         off_brush_right();
 491                         off_bagr();
 492                         robot_trajectory_new(NULL);
 493                         robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.8, 1.0);
 494                         robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M - 0.6,
 495                                                                0.4, TURN(DEG2RAD(90)));
 496                         /*FSM_TIMER(GO_TO_CONTAINER_TIMER);*/
 497                         break;
 498                 case EV_TIMER:
 499                 case EV_MOTION_DONE:
 500                         FSM_TRANSITION(go_back_to_edge);
 501                         break;
 502                 default: break;
 503         }
 504 }
 505
 506 FSM_STATE(go_back_to_edge)
 507 {
 508         switch (FSM_EVENT) {
 509                 case EV_ENTRY:
 510                         robot_move_by(-0.4, NO_TURN(), NULL);
 511                         break;
 512                 case EV_TIMER:
 513                 case EV_MOTION_DONE:
 514                         if (closed_to_container()) {
 515                                 robot.carousel_pos = 0;
 516                                 FSM_TRANSITION(deposite_balls);
 517                         } else {
 518                                 DBG("FIXME: go_closer_to_container\n");
 519                         }
 520                         break;
 521                 default: break;
 522         }
 523 }
 524
 525 /************************************************************************
 526  * BALLS AND CAROUSEL MANIPULATION STATES
 527  ************************************************************************/
 528
 529 #define MAX_GET_BALL_ATTEMPS    4
 530 #define GET_BALL_TIMER          1500
 531 #define WAIT_BALL_INSIDE        5000
 532 #define GET_BALL_BAGR_SPEED     130
 533
 534 FSM_STATE(get_balls)
 535 {
 536         static int get_ball_attemps = 0;
 537         static int ball_inside = 0;
 538         static int last_get = 0;
 539
 540         switch (FSM_EVENT) {
 541                 case EV_ENTRY:
 542                 case EV_RETURN:
 543                         if (last_get) {
 544                                 robot_move_by(-0.2, NO_TURN(), NULL);
 545                                 last_get = 0;
 546                         } else {
 547                                 get_ball_attemps = 0;
 548                                 DBG("balls_to_collect = %d\n", balls_to_collect);
 549                                 open_bottom_door();
 550                                 set_bagr(GET_BALL_BAGR_SPEED);
 551                                 brushes_drives_in();
 552                                 FSM_TIMER(500);
 553                         }
 554                         break;
 555                 case EV_TIMER:
 556                         if (ball_inside) {
 557                                 ball_inside = 0;
 558                                 close_bottom_door();
 559                                 off_bagr();
 560                                 if (robot.carousel_cnt >= balls_to_collect) {
 561                                         last_get = 1;
 562                                 }
 563                                 SUBFSM_TRANSITION(
 564                                         next_carousel_position, NULL);
 565                                 robot.carousel_pos =
 566                                         (robot.carousel_pos+2) % CAROUSEL_SIZE;
 567                         } else if (get_ball_attemps++ < MAX_GET_BALL_ATTEMPS) {
 568                                 FSM_TIMER(GET_BALL_TIMER);
 569                         } else {
 570                                 robot_move_by(-0.2, NO_TURN(), NULL);
 571                         }
 572                         break;
 573                 case EV_BALL_INSIDE:
 574                         /* ball is already inside */
 575                         if (ball_inside)
 576                                 break;
 577
 578                         ball_inside = 1;
 579                         robot.carousel[robot.carousel_pos % CAROUSEL_SIZE] =
 580                                 (enum ball_color)robot.cmu.color;
 581                         robot.carousel_cnt++;
 582                         DBG("collected balls = %d\n", robot.carousel_cnt);
 583                         FSM_TIMER(WAIT_BALL_INSIDE);
 584                         break;
 585                 case EV_MOTION_DONE:
 586                         FSM_TRANSITION(decide_where_now);
 587                         break;
 588                 default: break;
 589         }
 590 }
 591
 592 #define MAX_CAROUSEL_ATTEMPTS   3
 593 #define CAROUSEL_ATTEMPT_TIMER  1000
 594
 595 FSM_STATE(next_carousel_position)
 596 {
 597         static int carousel_attempts;
 598         int rv;
 599
 600         switch (FSM_EVENT) {
 601                 case EV_ENTRY:
 602                         carousel_attempts = 0;
 603                         DBG("carousel_pos = %d\n", robot.carousel_pos);
 604                         set_carousel(robot.carousel_pos);
 605                         FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
 606                         break;
 607                 case EV_TIMER:
 608                         carousel_attempts++;
 609                         /* check out, if the carousel reached position */
 610                         ROBOT_LOCK(drives);
 611                         rv = (robot.drives.carousel_pos == robot.carousel_pos);
 612                         ROBOT_UNLOCK(drives);
 613                         if (!rv && carousel_attempts++ < MAX_CAROUSEL_ATTEMPTS) {
 614                                 FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
 615                         } else if(rv) {
 616                                 DBG("carousel reached the position.\n");
 617                                 SUBFSM_RET(NULL);
 618                         } else {
 619                                 DBG("FIXME: carousel lost.\n");
 620                                 SUBFSM_RET(NULL);
 621                         }
 622                         break;
 623                 default: break;
 624         }
 625 }
 626
 627 #define WAIT_FOR_DEPOSITION_TIMER       1500
 628
 629 FSM_STATE(deposite_balls)
 630 {
 631         switch (FSM_EVENT) {
 632                 case EV_ENTRY:
 633                         if (robot.carousel_pos < CAROUSEL_SIZE) {
 634                                 close_back_door();
 635                                 SUBFSM_TRANSITION(next_carousel_position, NULL);
 636                         } else {
 637                                 robot.carousel_cnt = 0;
 638                                 robot.carousel_pos = 0;
 639                                 DBG("carousel_cnt = %d\n", robot.carousel_cnt);
 640                                 FSM_TRANSITION(decide_where_now);
 641                         }
 642                         break;
 643                 case EV_RETURN:
 644                         open_back_door();
 645                         FSM_TIMER(WAIT_FOR_DEPOSITION_TIMER);
 646                         break;
 647                 case EV_TIMER:
 648                         robot.carousel_pos++;
 649                         close_back_door();
 650                         FSM_TRANSITION(deposite_balls);
 651                         break;
 652                 default: break;
 653         }
 654 }
 655
 656 /* main loop */
 657 int main()
 658 {
 659         /* robot initialization */
 660         robot_init();
 661
 662         robot.carousel_cnt = 0;
 663         robot.carousel_pos = 0;
 664
 665         FSM(MAIN)->debug_states = 1;
 666         FSM(MOTION)->debug_states = 1;
 667
 668         robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
 669         robot.fsm[FSM_ID_MAIN].transition_callback = trans_callback;
 670         robot.fsm[FSM_ID_MOTION].transition_callback = move_trans_callback;
 671
 672         /* start threads and wait */
 673         robot_start();
 674
 675         robot_wait();
 676
 677         /* clean up */
 678         robot_destroy();
 679
 680         return 0;
 681 }