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