src: raw code a changes

[eurobot/public.git] / src / robofsm / fsmmove.cc

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index 2b810f26761127b9deb8b75226f10faf16495886..4a10a77bea8c7f0c19fef6eb50c133bf297362d9 100644 (file)
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -25,6 +25,7 @@
 #include "motion-control.h"
 #include <hokuyo.h>
 #include <ul_log.h>
+#include "../pathplan/aalgorithm.h"
 
 UL_LOG_CUST(ulogd_fsmmove); /* Log domain name = ulogd + name of the file */
 
@@ -66,7 +67,7 @@ static double free_space_in_front_of_robot()
                if (m > 19 && m < min)
                        min = m;
        }
-       return min/1000.0 - HOKUYO_CENTER_OFFSET_M;;
+       return min/1000.0 - (ROBOT_AXIS_TO_FRONT_M - HOKUYO_CENTER_OFFSET_M);
 }
 
 static bool obstackle_in_front_if_turn(Trajectory *t)
@@ -278,6 +279,7 @@ FSM_STATE_DECL(wait_and_try_again);
 FSM_STATE(init)
 {
        if (FSM_EVENT == EV_ENTRY) {
+               init_aalgorithm();
                FSM_TRANSITION(wait_for_command);
        }
 }
@@ -328,6 +330,7 @@ FSM_STATE(movement)
                        FSM_TRANSITION(wait_for_command);
                        break;
                case EV_OBSTACLE:
+                       stop();
                        switch (recalculate_trajectory()) {
                                // We can go to the target:
                                case TARGET_OK:    break;