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