Trajectory controller uses odometry and uzv only just before movement start

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

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index bdf4f1d1767fec73e707cb492468499efc0b667c..50a11b0a1bb95e6389458df631be3fcdb467a391 100644 (file)
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -285,6 +285,12 @@ FSM_STATE(wait_for_command)
 FSM_STATE(movement)
 {
        switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       // TODO lock
+                       robot_set_est_pos_notrans(robot.est_pos_uzv.x,
+                                                 robot.est_pos_uzv.y,
+                                                 robot.est_pos_uzv.phi);
+                       break;
                case EV_TRAJECTORY_DONE:
                        // Skip close to target because sometimes it turn the robot to much
 //                     FSM_TRANSITION(close_to_target);
@@ -309,7 +315,6 @@ FSM_STATE(movement)
                case EV_MOVE_STOP:
                        FSM_TRANSITION(wait_for_command);
                        break;
-               case EV_ENTRY:
                case EV_EXIT:
                        break;
                case EV_NEW_TARGET: