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