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 /************************************************************************
 217  * FSM STATES DECLARATION
 218  ************************************************************************/
 219
 220 /* initial and starting states */
 221 FSM_STATE_DECL(init);
 222 FSM_STATE_DECL(wait_for_start);
 223 /* movement states */
 224 FSM_STATE_DECL(go_to_our_white_dispenser);
 225 FSM_STATE_DECL(go_to_our_white_dispenser2);
 226 FSM_STATE_DECL(go_far_from_dispenser);
 227 FSM_STATE_DECL(go_from_our_dispenser);
 228 FSM_STATE_DECL(go_to_container);
 229 /* ball and carousel states */
 230 FSM_STATE_DECL(get_balls);
 231 FSM_STATE_DECL(next_carousel_position);
 232 FSM_STATE_DECL(deposite_balls);
 233
 234 /************************************************************************
 235  * INITIAL AND STARTING STATES
 236  ************************************************************************/
 237
 238 FSM_STATE(init)
 239 {
 240         /* start event ocurred */
 241         switch (FSM_EVENT) {
 242                 case EV_ENTRY:
 243                         robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
 244                                                 PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
 245                                                 DEG2RAD(135));
 246 //                      robot_set_est_pos_trans(0.15, PLAYGROUND_HEIGHT_M - 0.7, DEG2RAD(180));
 247                         off_brush_left();
 248                         off_brush_right();
 249                         brushes_out();
 250                         open_bottom_door();
 251                         close_back_door();
 252                         FSM_TRANSITION(wait_for_start);
 253 //                      FSM_TRANSITION(go_to_container);
 254 //                      FSM_TRANSITION(get_balls);
 255                         /* FIXME: delete after the test */
 256 //                      robot.carousel_cnt = 5;
 257 //                      robot.carousel_pos = 0;
 258 //                      FSM_TRANSITION(deposite_balls);
 259                         break;
 260                 default: break;
 261         }
 262 }
 263
 264 FSM_STATE(wait_for_start)
 265 {
 266         switch (FSM_EVENT) {
 267 #ifdef WAIT_FOR_START
 268                 case EV_START: {
 269                         pthread_t thid;
 270
 271                         /* start competition timer */
 272                         pthread_create(&thid, NULL, wait_for_end, NULL);
 273                         pthread_create(&thid, NULL, wait_to_deposition, NULL);
 274                 }
 275 #else
 276                 case EV_ENTRY:
 277 #endif
 278                         /* start to do something */
 279                         FSM_TRANSITION(go_to_our_white_dispenser);
 280                         break;
 281                 default: break;
 282         }
 283 }
 284
 285 /************************************************************************
 286  * MOVEMENT STATES
 287  ************************************************************************/
 288
 289 FSM_STATE(go_to_our_white_dispenser)
 290 {
 291         switch (FSM_EVENT) {
 292                 case EV_ENTRY:
 293                         robot_trajectory_new_backward(NULL);
 294                         robot_trajectory_add_final_point_trans(0.7,
 295                                         PLAYGROUND_HEIGHT_M - 0.65, NO_TURN());
 296                         break;
 297                 case EV_MOTION_DONE:
 298                         FSM_TRANSITION(go_to_our_white_dispenser2);
 299                         break;
 300                 default: break;
 301         }
 302 }
 303
 304 FSM_STATE(go_to_our_white_dispenser2)
 305 {
 306         struct ref_pos_type des_pos = {0.20, PLAYGROUND_HEIGHT_M - 0.65, 0};
 307         static int approaching_attempts = 0;
 308
 309         switch (FSM_EVENT) {
 310                 case EV_ENTRY:
 311                         robot_goto_point(des_pos);
 312                         break;
 313                 case EV_MOTION_DONE:
 314                         if (closed_to_dispenser() || approaching_attempts++ < 2) {
 315                                 FSM_TRANSITION(go_from_our_dispenser);
 316                         } else {
 317                                 FSM_TRANSITION(get_balls);
 318                                 approaching_attempts = 0;
 319                         }
 320                         break;
 321                 default: break;
 322         }
 323 }
 324
 325 FSM_STATE(go_to_our_red_dispenser)
 326 {
 327         switch (FSM_EVENT) {
 328                 case EV_ENTRY:
 329                         robot_trajectory_new_backward(NULL);
 330                         robot_trajectory_add_final_point_trans(0.7,
 331                                         PLAYGROUND_HEIGHT_M - 0.65, NO_TURN());
 332                         break;
 333                 case EV_MOTION_DONE:
 334                         FSM_TRANSITION(go_to_our_white_dispenser2);
 335                         break;
 336                 default: break;
 337         }
 338 }
 339
 340 FSM_STATE(go_to_our_red_dispenser2)
 341 {
 342         struct ref_pos_type des_pos = {0.65, PLAYGROUND_HEIGHT_M - 0.20, 0};
 343         static int approaching_attempts = 0;
 344
 345         switch (FSM_EVENT) {
 346                 case EV_ENTRY:
 347                         robot_goto_point(des_pos);
 348                         break;
 349                 case EV_MOTION_DONE:
 350                         if (closed_to_dispenser() || approaching_attempts++ < 2) {
 351                                 FSM_TRANSITION(go_from_our_dispenser);
 352                         } else {
 353                                 FSM_TRANSITION(get_balls);
 354                                 approaching_attempts = 0;
 355                         }
 356                         break;
 357                 default: break;
 358         }
 359 }
 360
 361 FSM_STATE(go_far_from_dispenser)
 362 {
 363         struct ref_pos_type des_pos = { 0.12, PLAYGROUND_HEIGHT_M - 0.65, 0};
 364
 365         switch (FSM_EVENT) {
 366                 case EV_ENTRY:
 367                         robot_goto_point(des_pos);
 368                         break;
 369                 case EV_MOTION_DONE:
 370                         FSM_TRANSITION(get_balls);
 371                         break;
 372                 default: break;
 373         }
 374 }
 375
 376 FSM_STATE(go_from_our_dispenser)
 377 {
 378         switch (FSM_EVENT) {
 379                 case EV_ENTRY:
 380                         /* FIXME: go backward by */
 381                         break;
 382                 case EV_MOTION_DONE:
 383                         FSM_TRANSITION(go_to_our_white_dispenser2);
 384                         break;
 385                 default: break;
 386         }
 387 }
 388
 389 #define GO_TO_CONTAINER_TIMER   10000
 390
 391 FSM_STATE(go_to_container)
 392 {
 393         switch (FSM_EVENT) {
 394                 case EV_ENTRY:
 395                         off_brush_left();
 396                         off_brush_right();
 397                         off_bagr();
 398                         robot_trajectory_new_backward(NULL);
 399                         robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.6, 1.0);
 400                         robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M - 0.4,
 401                                                 0, NO_TURN());
 402                         FSM_TIMER(GO_TO_CONTAINER_TIMER);
 403                         break;
 404                 case EV_TIMER:
 405                 case EV_MOTION_DONE:
 406                         if (closed_to_container()) {
 407                                 robot.carousel_pos = 0;
 408                                 FSM_TRANSITION(deposite_balls);
 409                         } else {
 410                                 DBG("FIXME: go_closer_to_container\n");
 411                         }
 412                         break;
 413                 default: break;
 414         }
 415 }
 416
 417 /************************************************************************
 418  * BALLS AND CAROUSEL MANIPULATION STATES
 419  ************************************************************************/
 420
 421 #define MAX_GET_BALL_ATTEMPS    5
 422 #define GET_BALL_TIMER          1500
 423 #define WAIT_BALL_INSIDE        5000
 424 #define GET_BALL_BAGR_SPEED     120
 425
 426 FSM_STATE(get_balls)
 427 {
 428         static int get_ball_attemps = 0;
 429         static int ball_inside = 0;
 430
 431         switch (FSM_EVENT) {
 432                 case EV_ENTRY:
 433                 case EV_RETURN:
 434                         open_bottom_door();
 435                         set_bagr(GET_BALL_BAGR_SPEED);
 436                         brushes_drives_in();
 437                         FSM_TIMER(500);
 438                         break;
 439                 case EV_TIMER:
 440                         if (ball_inside) {
 441                                 ball_inside = 0;
 442                                 close_bottom_door();
 443                                 off_bagr();
 444                                 if (robot.carousel_cnt >= BALL_TO_COLLECT) {
 445                                         /* FIXME: just to test deposition */
 446                                         robot.carousel_pos = 0;
 447                                         FSM_TRANSITION(deposite_balls);
 448 //                                      printf("go_to_container\n");
 449 //                                      FSM_TRANSITION(go_to_container);
 450                                 } else {
 451                                         robot.carousel_pos =
 452                                                 (robot.carousel_pos+2) % CAROUSEL_SIZE;
 453                                         SUBFSM_TRANSITION(
 454                                                 next_carousel_position, NULL);
 455                                 }
 456                         } else if (get_ball_attemps++ < MAX_GET_BALL_ATTEMPS) {
 457                                 FSM_TIMER(GET_BALL_TIMER);
 458                         } else {
 459                                 /* FIXME: */
 460                                 DBG("go_to_container\n");
 461 //                              FSM_TRANSITION(go_to_container);
 462                         }
 463                         break;
 464                 case EV_BALL_INSIDE:
 465                         /* ball is already inside */
 466                         if (ball_inside)
 467                                 break;
 468
 469                         ball_inside = 1;
 470                         robot.carousel[robot.carousel_pos % CAROUSEL_SIZE] =
 471                                 (enum ball_color)robot.cmu.color;
 472                         robot.carousel_cnt++;
 473                         DBG("ball cnt=%d\n", robot.carousel_cnt);
 474                         FSM_TIMER(WAIT_BALL_INSIDE);
 475                         break;
 476                 default: break;
 477         }
 478 }
 479
 480 #define MAX_CAROUSEL_ATTEMPTS   3
 481 #define CAROUSEL_ATTEMPT_TIMER  1000
 482
 483 FSM_STATE(next_carousel_position)
 484 {
 485         static int carousel_attempts;
 486         int rv;
 487
 488         switch (FSM_EVENT) {
 489                 case EV_ENTRY:
 490                         carousel_attempts = 0;
 491                         DBG("carousel_pos = %d\n", robot.carousel_pos);
 492                         set_carousel(robot.carousel_pos);
 493                         FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
 494                         break;
 495                 case EV_TIMER:
 496                         carousel_attempts++;
 497                         /* check out, if the carousel reached position */
 498                         ROBOT_LOCK(drives);
 499                         rv = (robot.drives.carousel_pos == robot.carousel_pos);
 500                         ROBOT_UNLOCK(drives);
 501                         if (!rv && carousel_attempts++ < MAX_CAROUSEL_ATTEMPTS) {
 502                                 FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
 503                         } else if(rv) {
 504                                 DBG("carousel reached the position.\n");
 505                                 SUBFSM_RET(NULL);
 506                         } else {
 507                                 DBG("FIXME: carousel lost.\n");
 508                                 SUBFSM_RET(NULL);
 509                         }
 510                         break;
 511                 default: break;
 512         }
 513 }
 514
 515 #define WAIT_FOR_DEPOSITION_TIMER       1500
 516
 517 FSM_STATE(deposite_balls)
 518 {
 519         switch (FSM_EVENT) {
 520                 case EV_ENTRY:
 521                         if (robot.carousel_pos < CAROUSEL_SIZE) {
 522                                 close_back_door();
 523                                 SUBFSM_TRANSITION(next_carousel_position, NULL);
 524                         } else {
 525                                 robot.carousel_cnt = 0;
 526                                 /* FIXME: */
 527                                 DBG("go_to_our_white_dispenser\n");
 528 //                              FSM_TRANSITION(go_to_our_white_dispenser);
 529                         }
 530                         break;
 531                 case EV_RETURN:
 532                         DBG("open_back_door\n");
 533                         open_back_door();
 534                         FSM_TIMER(WAIT_FOR_DEPOSITION_TIMER);
 535                         break;
 536                 case EV_TIMER:
 537                         robot.carousel_cnt--;
 538                         robot.carousel_pos++;
 539                         close_back_door();
 540                         DBG("carousel_cnt = %d\n", robot.carousel_cnt);
 541                         FSM_TRANSITION(deposite_balls);
 542                         break;
 543                 default: break;
 544         }
 545 }
 546
 547 /* main loop */
 548 int main()
 549 {
 550         /* robot initialization */
 551         robot_init();
 552
 553         robot.carousel_cnt = 0;
 554         robot.carousel_pos = 0;
 555
 556         FSM(MAIN)->debug_states = 1;
 557
 558         robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
 559
 560         /* start threads and wait */
 561         robot_start();
 562
 563         robot_wait();
 564
 565         /* clean up */
 566         robot_destroy();
 567
 568         return 0;
 569 }