src/robofsm/fsmmove.cc

   1 /**
   2  * @file        fsmmove.cc
   3  * @brief       FSM move
   4  * @author      Michal Sojka, Jose Maria Martin Laguna, Petr Beneš
   5  *
   6  */
   7 #define NEVER
   8 #define DEBUG
   9 #define FSM_MOTION
  10 #include <sys/time.h>
  11 #include <time.h>
  12 #include "trgen.h"
  13 #include "balet.h"
  14 #include <robodata.h>
  15 #include <robot.h>
  16 #include <pthread.h>
  17 #include <path_planner.h>
  18 #include <signal.h>
  19 #include <movehelper.h>
  20 #include <sharp.h>
  21 #include <unistd.h>
  22 #include <map.h>
  23 #include "robot_config.h"
  24 #include <robomath.h>
  25
  26 /*******************************************************************************
  27  * Controller thread and helper functions for that thread
  28  *******************************************************************************/
  29
  30
  31 //Controller gains
  32 const struct balet_params k = {
  33   p_tangent:  2,                // dx gain
  34   p_angle: 1.5,                         // dphi gain
  35   p_perpen: 10                  // dy gain
  36 };
  37
  38 #define MAX_POS_ERROR_M 0.5
  39 #define CLOSE_TO_TARGET_M 0.1
  40 #define MAX_WAIT_FOR_CLOSE_MS 2000
  41
  42 //#define MOTION_PERIOD_NS (500/*ms*/*1000*1000)
  43 #define MOTION_PERIOD_NS (50/*ms*/*1000*1000)
  44 #define SIG_TIMER (SIGRTMIN+0)
  45 #define SIG_NEWDATA (SIGRTMIN+1)
  46
  47 // time to calculate new trajectory
  48 #define TRGEN_DELAY 0.3
  49
  50 // Global varibles
  51 static pthread_t thr_trajectory_follower;
  52 static struct timeval tv_start; /**< Absolute time, when trajectory started. */
  53
  54 // Trajectory recalculation thread and synchoronization
  55 pthread_t thr_trajctory_recalculation;
  56 sem_t start_recalculation;
  57 sem_t recalculation_not_running;
  58
  59 /** Stores the actually followed trajectory object */
  60 static Trajectory *actual_trajectory;
  61 bool actual_trajectory_reverse;
  62 pthread_mutex_t actual_trajectory_lock = PTHREAD_MUTEX_INITIALIZER;
  63 pthread_mutex_t switch_to_trajectory_lock = PTHREAD_MUTEX_INITIALIZER;
  64
  65
  66 // Target position of the current movement
  67 static struct move_target current_target;
  68
  69
  70 static Trajectory *switch_to_trajectory;
  71 static double switch_time;
  72 static bool recalculation_running;
  73
  74 static void stop();
  75 static void go(Trajectory *t);
  76
  77 static void delete_actual_trajectory()
  78 {
  79         Trajectory *old;
  80         pthread_mutex_lock(&actual_trajectory_lock);
  81         old = actual_trajectory;
  82         actual_trajectory = NULL;
  83         pthread_mutex_unlock(&actual_trajectory_lock);
  84         robot_send_speed(0,0);
  85         if (old) delete(actual_trajectory);
  86 }
  87
  88 /** Sends events from follower thread to FSM. */
  89 static void notify_fsm(bool done, double error)
  90 {
  91         static bool done_sent;
  92
  93         if (error > MAX_POS_ERROR_M) {
  94             FSM_SIGNAL(MOTION, EV_TRAJECTORY_LOST, NULL);
  95         } else {
  96                 if (done) {
  97                         if (error < CLOSE_TO_TARGET_M) {
  98                                 FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE_AND_CLOSE, NULL);
  99                         } else if (!done_sent) {
 100                                 done_sent = true;
 101                                 FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE, NULL);
 102                         }
 103                 } else {
 104                         done_sent = false;
 105                 }
 106         }
 107 }
 108
 109 static void check_for_collision_in_future(Trajectory *traj, double current_time)
 110 {
 111         Pos future_pos;
 112         struct map *map = robot.map;
 113         int xcell, ycell;
 114         double x, y;
 115         bool valid;
 116         unsigned i;
 117 //      const double times[] = { 0.5, 0.3, 0.1 }; // seconds
 118         const double times[] = { 0.7, 0.5, 0.4, 0.3 }; // seconds
 119
 120
 121         for (i=0; i < sizeof(times)/sizeof(times[0]); i++) {
 122                 traj->getRefPos(current_time+times[i], future_pos);
 123
 124                 /* Ignore obstacles when turning */
 125                 if (fabs(future_pos.v) < 0.01)
 126                         continue;
 127
 128                 x = future_pos.x + cos(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
 129                 y = future_pos.y + sin(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
 130
 131                 ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
 132                 if (!valid)
 133                         continue;
 134                 if (map->cells[ycell][xcell].detected_obstacle > 0) {
 135                         if (sem_trywait(&recalculation_not_running) == 0) {
 136                                 sem_post(&start_recalculation);
 137                                 break;
 138                         }
 139                 }
 140         }
 141 }
 142
 143 static void do_control()
 144 {
 145         double speedl, speedr;
 146
 147         double t;
 148         struct timeval tv;
 149
 150         // Calculate reference position
 151         gettimeofday(&tv, NULL);
 152         t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 153         t += (tv.tv_sec - tv_start.tv_sec);
 154
 155     // check for new trajectory to switch
 156     // only if the trajectory is already prepared
 157     if (switch_to_trajectory != NULL && t >= switch_time) {
 158         pthread_mutex_lock(&switch_to_trajectory_lock);
 159 #ifdef NEVER // DEBUG
 160         printf("SWITCHING to new trajectory\n");
 161         fflush(stdout);
 162 #endif
 163         go(switch_to_trajectory);
 164         // nothing prepared now
 165         switch_to_trajectory = NULL;
 166         pthread_mutex_unlock(&switch_to_trajectory_lock);
 167         }
 168
 169         pthread_mutex_lock(&actual_trajectory_lock);
 170         Trajectory *w = actual_trajectory;
 171         if (w) {
 172                 Pos ref_pos, est_pos, balet_out;
 173                 bool done;
 174
 175         // Calculate reference position
 176         gettimeofday(&tv, NULL);
 177         t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 178         t += (tv.tv_sec - tv_start.tv_sec);
 179
 180         // if switch_to_trajectory is being prepared, it can not stop calculation
 181         // and start to count again, it could evoke overloading
 182         if (robot.obstacle_avoidance_enabled && !recalculation_running && switch_to_trajectory == NULL)
 183                     check_for_collision_in_future(w, t);
 184
 185
 186                 done = w->getRefPos(t, ref_pos);
 187
 188         if (ref_pos.omega > actual_trajectory->constr.maxomega)
 189             printf("Omega constraint problem %lf, max %lf -------------------- \n", ref_pos.omega, actual_trajectory->constr.maxomega);
 190
 191                 ROBOT_LOCK(ref_pos);
 192                 robot.ref_pos.x = ref_pos.x;
 193                 robot.ref_pos.y = ref_pos.y;
 194                 robot.ref_pos.phi = ref_pos.phi;
 195                 ROBOT_UNLOCK(ref_pos);
 196
 197                 ROBOT_LOCK(est_pos);
 198                 est_pos.x = robot.est_pos.x;
 199                 est_pos.y = robot.est_pos.y;
 200                 est_pos.phi = robot.est_pos.phi;
 201                 ROBOT_UNLOCK(est_pos);
 202
 203 #ifdef CONFIG_OPEN_LOOP
 204                 // We don't use any feedback now. It is
 205                 // supposed that the estimated position is
 206                 // equal to the reference position.
 207                 robot_set_est_pos_notrans(ref_pos.x, ref_pos.y, ref_pos.phi);
 208                 est_pos.x = ref_pos.x;
 209                 est_pos.y = ref_pos.y;
 210                 est_pos.phi = ref_pos.phi;
 211 #endif
 212
 213 #ifdef NEVER //DEBUG
 214                 printf("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f  omega=%-4.02f, time=%lf \r", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v, ref_pos.omega, t);
 215                 fflush(stdout);
 216 #endif
 217                 // Call the controller
 218                 double error;
 219                 error = balet(ref_pos, est_pos, k, balet_out);
 220                 speedl = balet_out.v - ROBOT_ROTATION_RADIUS_M*balet_out.omega;
 221                 speedr = balet_out.v + ROBOT_ROTATION_RADIUS_M*balet_out.omega;
 222                 notify_fsm(done, error);
 223         } else {
 224                 speedl = 0;
 225                 speedr = 0;
 226         }
 227
 228
 229         // Apply controller output
 230         robot_send_speed(speedl, speedr);
 231         pthread_mutex_unlock(&actual_trajectory_lock);
 232 }
 233
 234 /**
 235  * A thread running the controller.
 236  *
 237  * This (high priority) thread executes the motion control
 238  * algorithm. It calculates repference position based on actual
 239  * trajectory and current time. Then it calls "balet" controller to
 240  * close feedback.
 241  *
 242  * @param arg
 243  *
 244  * @return
 245  */
 246 static void *thread_trajectory_follower(void *arg)
 247 {
 248         struct itimerspec ts;
 249         sigset_t sigset;
 250         struct sigevent sigevent;
 251         timer_t timer;
 252
 253         ts.it_value.tv_sec = 0;
 254         ts.it_value.tv_nsec = MOTION_PERIOD_NS;
 255         ts.it_interval.tv_sec = 0;
 256         ts.it_interval.tv_nsec = MOTION_PERIOD_NS;
 257
 258         sigemptyset(&sigset);
 259         sigaddset(&sigset, SIG_TIMER);
 260         sigaddset(&sigset, SIG_NEWDATA);
 261
 262         pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*) NULL);
 263
 264         sigevent.sigev_notify = SIGEV_SIGNAL;
 265         sigevent.sigev_signo = SIG_TIMER;
 266         sigevent.sigev_value.sival_int = 0;
 267
 268         if (timer_create(CLOCK_REALTIME, &sigevent, &timer) < 0) {
 269                 perror("move: timer_create");
 270                 return NULL;
 271         }
 272
 273         if (timer_settime(timer, 0, &ts, NULL) < 0) {
 274                 perror("move: timer_settimer");
 275                 return NULL;
 276         }
 277
 278         while (1) {
 279                 int sig;
 280                 // Wait for start of new period or new data
 281                 sigwait(&sigset, &sig);
 282                 //DBG("signal %d ", sig - SIGRTMIN);
 283                 do_control();
 284         }
 285 }
 286
 287 /**
 288  * Tells trajctory_follower to start moving along trajectory @c t.
 289  *
 290  * @param t Trajectory to follow.
 291  */
 292 static void go(Trajectory *t)
 293 {
 294         pthread_mutex_lock(&actual_trajectory_lock);
 295         Trajectory *old;
 296         old = actual_trajectory;
 297         gettimeofday(&tv_start, NULL);
 298         actual_trajectory = t;
 299         pthread_mutex_unlock(&actual_trajectory_lock);
 300         if (old)
 301                 delete(old);
 302
 303 }
 304
 305 /**
 306  * switches to newly counted trajectory to go on it a specific time
 307  */
 308 static void switch_trajectory_at(Trajectory *t, double time)
 309 {
 310         pthread_mutex_lock(&switch_to_trajectory_lock);
 311     switch_to_trajectory = t;
 312     switch_time = time;
 313         pthread_mutex_unlock(&switch_to_trajectory_lock);
 314
 315     struct timeval tv;
 316         gettimeofday(&tv, NULL);
 317         double tm = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 318         tm += (tv.tv_sec - tv_start.tv_sec);
 319     if (switch_time <= tm)
 320         printf("*/*/*/*/* BAD SWITCH /*/*/*/*");
 321 }
 322
 323
 324 static void stop()
 325 {
 326         delete_actual_trajectory();
 327         pthread_kill(thr_trajectory_follower, SIG_NEWDATA);
 328 }
 329
 330 enum target_status {
 331         TARGET_OK,
 332         TARGET_NOT_REACHABLE,
 333         TARGET_ERROR
 334 };
 335 /**
 336  * @brief       Calculates and simplify a path to goal position avoiding obstacles.
 337  * @param start_in_future is delay utilised to count the trajectory while robot
 338  * is moving (zero means first to stop robot and then count)
 339  * @return      True on succes, false on error.
 340  */
 341 static enum target_status new_goal(struct move_target *target, double start_in_future)
 342 {
 343         enum target_status ret;
 344         double startx, starty, angle;
 345         PathPoint * path = NULL;
 346         PathPoint * tmp_point = NULL;
 347     recalculation_running = true;
 348
 349     if(!actual_trajectory)
 350         {
 351         start_in_future = 0;
 352         }
 353
 354         struct TrajectoryConstraints tc;
 355         Pos start;
 356         //tc.maxv *= 2.5;
 357         //tc.maxacc *= 2.5;
 358
 359     double time_ts;
 360
 361     if (!start_in_future)
 362     {
 363         /// Get actual position.
 364         ROBOT_LOCK(est_pos);
 365         start.x = robot.est_pos.x;
 366         start.y = robot.est_pos.y;
 367         start.phi = robot.est_pos.phi;
 368         start.v = 0;
 369         start.omega = 0;
 370         ROBOT_UNLOCK(est_pos);
 371     }
 372     else
 373     {
 374                 struct timeval tv;
 375                 gettimeofday(&tv, NULL);
 376                 time_ts = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 377                 time_ts += (tv.tv_sec - tv_start.tv_sec);
 378         time_ts += start_in_future;
 379         //position in the future
 380         pthread_mutex_lock(&actual_trajectory_lock);
 381         actual_trajectory->getRefPos(time_ts, start);
 382         pthread_mutex_unlock(&actual_trajectory_lock);
 383     }
 384         tc = target->tc;
 385
 386         //DBG("Going from (%.2f, %.2f) to (%.2f, %.2f)\n", startx, starty, goalx, goaly);
 387         /// Calculates the path between actual position to goal position. If the goal is not reachable returns -1.
 388         startx = start.x;
 389         starty = start.y;
 390         switch (path_planner(startx, starty, target->x, target->y, &path, &angle)) {
 391                 case PP_ERROR_MAP_NOT_INIT:
 392                         ret = TARGET_ERROR; break;
 393                 case PP_ERROR_GOAL_IS_OBSTACLE:
 394                 case PP_ERROR_GOAL_NOT_REACHABLE:
 395                         ret = TARGET_NOT_REACHABLE; break;
 396                 default:
 397                         ret = TARGET_OK; break;
 398         }
 399
 400         if (ret != TARGET_OK)
 401         {
 402         recalculation_running=false;
 403                 return ret;
 404         }
 405
 406         bool backward = false;
 407         Trajectory *t = new Trajectory(tc, backward);
 408
 409         // Add this  path to the trajectory.
 410         for (tmp_point = path; tmp_point!=NULL; tmp_point=tmp_point->next) {
 411                 DBG("ADDDING POINT (%f, %f)\n", tmp_point->x, tmp_point->y);
 412                 t->addPoint(tmp_point->x, tmp_point->y);
 413         }
 414
 415         t->finalHeading = target->h;
 416         freePathMemory(path);
 417
 418         if (t->prepare(start)) {
 419         if (!start_in_future) {
 420                 go(t);
 421             }
 422         else
 423             // not to switch immediately
 424             switch_trajectory_at(t, time_ts);
 425         } else {
 426                 delete(t);
 427                 ret = TARGET_ERROR;
 428         }
 429
 430     recalculation_running=false;
 431         return ret;
 432 }
 433
 434
 435 /**
 436  * Starts moving on trajectory prepared by functions in
 437  * movehelper.cc. In case of error it sends the proper event to MAIN
 438  * FSM.
 439  *
 440  * @return true on succes, false on error.
 441  */
 442 static enum target_status new_trajectory(Trajectory *t)
 443 {
 444         Pos pos;
 445
 446         if (!t) {
 447                 printf("ERROR: No trajectory\n");
 448                 return TARGET_ERROR;
 449         }
 450         ROBOT_LOCK(est_pos);
 451         pos.x = robot.est_pos.x;
 452         pos.y = robot.est_pos.y;
 453         pos.phi = robot.est_pos.phi;
 454         ROBOT_UNLOCK(est_pos);
 455
 456         if (t->prepare(pos)) {
 457                 go(t);
 458                 return TARGET_OK;
 459         } else {
 460                 delete(t);
 461                 return TARGET_ERROR;
 462         }
 463 }
 464
 465 static enum target_status new_target(struct move_target *target)
 466 {
 467         enum target_status ret;
 468         if (target->trajectory) {
 469                 Trajectory *t = (Trajectory*)target->trajectory;
 470                 ret = new_trajectory(t);
 471                 // Trajectory is deleted by somebody else
 472                 target->tc = t->constr;
 473                 target->trajectory = NULL;
 474         } else {
 475                 ret = new_goal(target, 0);
 476         }
 477         if (ret != TARGET_ERROR) {
 478                 current_target = *target;
 479         }
 480         return ret;
 481 }
 482
 483 /* This recalculation must only be triggered from mvoement state. */
 484 void *thread_trajectory_realculation(void *arg)
 485 {
 486         while (1) {
 487                 sem_wait(&start_recalculation);
 488                 /* TODO: Different start for planning than esitmated position */
 489                 enum target_status ret = new_goal(&current_target, TRGEN_DELAY);
 490                 switch (ret) {
 491                         case TARGET_OK:
 492                                 break;
 493                         case TARGET_ERROR:
 494                                 printf("Target error on recalculation_in_progress\n");
 495                                 FSM_SIGNAL(MOTION, EV_RECALCULATION_FAILED, NULL);
 496                                 break;
 497                         case TARGET_NOT_REACHABLE:
 498                 printf("----- TARGET_NOT_REACHABLE");
 499                                 FSM_SIGNAL(MOTION, EV_RECALCULATION_FAILED, NULL);
 500                                 break;
 501                 }
 502                 sem_post(&recalculation_not_running);
 503         }
 504 }
 505
 506 /*******************************************************************************
 507  * The automaton
 508  *******************************************************************************/
 509
 510 FSM_STATE_DECL(movement);
 511 FSM_STATE_DECL(close_to_target);
 512 FSM_STATE_DECL(wait_for_command);
 513 FSM_STATE_DECL(reset_mcl);
 514 FSM_STATE_DECL(wait_and_try_again);
 515
 516
 517 FSM_STATE(init)
 518 {
 519         pthread_attr_t tattr;
 520         sched_param param;
 521         int ret;
 522
 523         if (FSM_EVENT == EV_ENTRY) {
 524                 actual_trajectory = NULL;
 525         switch_to_trajectory = NULL;
 526
 527                 // Trajectory follower thread
 528                 pthread_attr_init (&tattr);
 529                 pthread_attr_getschedparam (&tattr, &param);
 530                 param.sched_priority = THREAD_PRIO_TRAJ_FOLLOWER;
 531                 ret = pthread_attr_setschedparam (&tattr, &param);
 532                 if(ret) {
 533                         perror("move_init: pthread_attr_setschedparam(follower)");
 534                         robot_exit();
 535                 }
 536                 ret = pthread_create(&thr_trajectory_follower, &tattr, thread_trajectory_follower, NULL);
 537                 if(ret) {
 538                         perror("move_init: pthread_create");
 539                         robot_exit();
 540                 }
 541
 542                 // Trajectory follower thread
 543                 sem_init(&recalculation_not_running, 0, 1);
 544                 sem_init(&start_recalculation, 0, 0);
 545                 pthread_attr_init (&tattr);
 546                 pthread_attr_getschedparam (&tattr, &param);
 547                 param.sched_priority = THREAD_PRIO_TRAJ_RECLAC;
 548                 ret = pthread_attr_setschedparam (&tattr, &param);
 549                 if(ret) {
 550                         perror("move_init: pthread_attr_setschedparam(follower)");
 551                         robot_exit();
 552                 }
 553                 ret = pthread_create(&thr_trajctory_recalculation, &tattr, thread_trajectory_realculation, NULL);
 554                 if(ret) {
 555                         perror("move_init: pthread_create");
 556                         robot_exit();
 557                 }
 558
 559                 FSM_TRANSITION(wait_for_command);
 560         }
 561 }
 562
 563 #define FIND_NEW_WAY(target)                                    \
 564 do {                                                            \
 565         enum target_status ret = new_goal(target, 0);           \
 566         switch (ret) {                                          \
 567                 case TARGET_OK:                                 \
 568                         FSM_TRANSITION(movement);               \
 569                         break;                                  \
 570                 case TARGET_ERROR:                              \
 571                         printf("Target error\n");               \
 572                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); \
 573                         break;                                  \
 574                 case TARGET_NOT_REACHABLE:                      \
 575                         FSM_TRANSITION(wait_and_try_again);     \
 576                         break;                                  \
 577         }                                                       \
 578 } while(0)
 579
 580
 581 FSM_STATE(wait_for_command)
 582 {
 583         enum target_status ret;
 584         switch (FSM_EVENT) {
 585                 case EV_ENTRY:
 586                         stop();
 587                         break;
 588                 case EV_NEW_TARGET:
 589                         ret = new_target((struct move_target*)FSM_EVENT_PTR);
 590                         switch (ret) {
 591                                 case TARGET_OK:
 592                                         FSM_TRANSITION(movement);
 593                                         break;
 594                                 case TARGET_ERROR:
 595                                         printf("Target error\n");
 596                                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 597                                         break;
 598                                 case TARGET_NOT_REACHABLE:
 599                                         FSM_TRANSITION(wait_and_try_again);
 600                                         break;
 601                         }
 602                         free(FSM_EVENT_PTR);
 603                         break;
 604                 default:
 605                         break;
 606         }
 607 }
 608
 609 FSM_STATE(movement)
 610 {
 611         switch (FSM_EVENT) {
 612                 case EV_TRAJECTORY_DONE:
 613                         FSM_TRANSITION(close_to_target);
 614                         break;
 615                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 616                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 617                         FSM_TRANSITION(wait_for_command);
 618                         break;
 619                 case EV_RECALCULATION_FAILED:
 620                         FSM_TRANSITION(wait_and_try_again);
 621                         break;
 622                 case EV_TRAJECTORY_LOST:
 623                         FSM_TRANSITION(reset_mcl);
 624                         break;
 625                 case EV_MOVE_STOP:
 626                         FSM_TRANSITION(wait_for_command);
 627                         break;
 628                 case EV_ENTRY:
 629                 case EV_EXIT:
 630                         break;
 631                 case EV_NEW_TARGET:
 632                 case EV_TIMER:
 633                 case EV_RETURN:
 634                 case EV_FOUND_AFTER_RESET:
 635                         DBG_PRINT_EVENT("unhandled event");
 636                         break;
 637         }
 638 }
 639
 640 FSM_STATE(close_to_target)
 641 {
 642         switch (FSM_EVENT) {
 643                 case EV_RETURN:
 644                         if (FSM_EVENT_INT == EV_MOVE_STOP)
 645                                 FSM_TRANSITION(wait_for_command);
 646                         //
 647                 case EV_ENTRY:
 648                         FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
 649                         break;
 650                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 651                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 652                         FSM_TRANSITION(wait_for_command);
 653                         break;
 654                 case EV_TRAJECTORY_LOST:
 655                 case EV_TIMER:
 656                         FSM_TRANSITION(reset_mcl);
 657                         break;
 658                 case EV_MOVE_STOP:
 659                         FSM_TRANSITION(wait_for_command);
 660                         break;
 661                 case EV_EXIT:
 662                         stop();
 663                         break;
 664                 case EV_TRAJECTORY_DONE:
 665                 case EV_NEW_TARGET:
 666                 case EV_FOUND_AFTER_RESET:
 667                 case EV_RECALCULATION_FAILED:
 668                         DBG_PRINT_EVENT("unhandled event");
 669                         break;
 670         }
 671 }
 672
 673 FSM_STATE(reset_mcl)
 674 {
 675         switch (FSM_EVENT) {
 676                 case EV_ENTRY:
 677                         stop();
 678                         //FSM_SIGNAL(LOC, EV_RESET, NULL);
 679                         break;
 680                 case EV_FOUND_AFTER_RESET:
 681                         FIND_NEW_WAY(&current_target);
 682                         break;
 683                 case EV_MOVE_STOP:
 684                         FSM_TRANSITION(wait_for_command);
 685                         break;
 686                 case EV_EXIT:
 687                         break;
 688                 case EV_TRAJECTORY_DONE:
 689                 case EV_NEW_TARGET:
 690                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 691                 case EV_TRAJECTORY_LOST:
 692                 case EV_RETURN:
 693                 case EV_TIMER:
 694                 case EV_RECALCULATION_FAILED:
 695                         DBG_PRINT_EVENT("unhandled event");
 696         }
 697 }
 698
 699
 700 FSM_STATE(wait_and_try_again)
 701 {
 702         switch (FSM_EVENT) {
 703                 case EV_ENTRY:
 704                         stop(); //FIXME: Not hard stop
 705                         FSM_TIMER(1000);
 706                         break;
 707                 case EV_TIMER:
 708                         FIND_NEW_WAY(&current_target);
 709                         break;
 710                 case EV_MOVE_STOP:
 711                         FSM_TRANSITION(wait_for_command);
 712                         break;
 713                 case EV_TRAJECTORY_DONE:
 714                 case EV_NEW_TARGET:
 715                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 716                 case EV_TRAJECTORY_LOST:
 717                 case EV_RETURN:
 718                 case EV_FOUND_AFTER_RESET:
 719                 case EV_RECALCULATION_FAILED:
 720                         DBG_PRINT_EVENT("unhandled event");
 721                         break;
 722                 case EV_EXIT:
 723                         break;
 724         }
 725 }