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