Trajectory controller uses odometry and uzv only just before movement start

[eurobot/public.git] / src / robofsm / motion-control.cc

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 068803a0c57919e7e4846a812f01ae0780905ac1..236bdade11cac7563da944949a0862d6f87cba84 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -317,9 +317,9 @@ static void do_control()
                ROBOT_UNLOCK(ref_pos);
 
                ROBOT_LOCK(est_pos_uzv);
-               est_pos.x = robot.est_pos_uzv.x;
-               est_pos.y = robot.est_pos_uzv.y;
-               est_pos.phi = robot.est_pos_uzv.phi;
+               est_pos.x = robot.est_pos_odo.x;
+               est_pos.y = robot.est_pos_odo.y;
+               est_pos.phi = robot.est_pos_odo.phi;
                ROBOT_UNLOCK(est_pos_uzv);
 
                if (measurement_ok < 2) {