robofsm: Added estimated position based on odometry

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

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 7ec91e1f5195e3bc0212b28f4a498eaed947b285..068803a0c57919e7e4846a812f01ae0780905ac1 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -241,25 +241,25 @@ static void do_estimation()
 
        if (virgo || init_ekf_flag) {
                /*FIXME:reflect init pos & beacon coords accord.to our color */
-               ROBOT_LOCK(est_pos);
-               real_t xy0[] = {robot.est_pos.x, robot.est_pos.y};
+               ROBOT_LOCK(est_pos_uzv);
+               real_t xy0[] = {robot.est_pos_uzv.x, robot.est_pos_uzv.y};
                y[3] = y[4] = 0.0;
                ekf8_init(&ekf8, (real_t*)beacon_xy, D_MAX, 30.0, xy0, y);
-               ekf8.ekf.x[6] = robot.est_pos.phi;
+               ekf8.ekf.x[6] = robot.est_pos_uzv.phi;
                init_ekf_flag = false;
                virgo = false;
-               ROBOT_UNLOCK(est_pos);
+               ROBOT_UNLOCK(est_pos_uzv);
        }
 
        ekf8_step(&ekf8, x, P, err, y);
 
        DBG("EKF: x=%f   y=%f   phi=%8.4f\n", x[0], x[1], x[6]*(180.0/M_PI));
 
-       ROBOT_LOCK(est_pos);
-       robot.est_pos.x = x[0] - ODO_D*cos(x[6]);
-       robot.est_pos.y = x[1] - ODO_D*sin(x[6]);
-       robot.est_pos.phi = x[6];
-       ROBOT_UNLOCK(est_pos);
+       ROBOT_LOCK(est_pos_uzv);
+       robot.est_pos_uzv.x = x[0] - ODO_D*cos(x[6]);
+       robot.est_pos_uzv.y = x[1] - ODO_D*sin(x[6]);
+       robot.est_pos_uzv.phi = x[6];
+       ROBOT_UNLOCK(est_pos_uzv);
 }
 
 static void do_control()
@@ -316,11 +316,11 @@ static void do_control()
                robot.ref_pos.phi = ref_pos.phi;
                ROBOT_UNLOCK(ref_pos);
 
-               ROBOT_LOCK(est_pos);
-               est_pos.x = robot.est_pos.x;
-               est_pos.y = robot.est_pos.y;
-               est_pos.phi = robot.est_pos.phi;
-               ROBOT_UNLOCK(est_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;
+               ROBOT_UNLOCK(est_pos_uzv);
 
                if (measurement_ok < 2) {
                        // We don't have any feedback now. It is
@@ -549,12 +549,12 @@ void get_future_pos(double rel_time_sec, Pos &pos, double &switch_time)
                // Robot doesn't move, so return current position
                pthread_mutex_unlock(&actual_trajectory_lock);
 
-               ROBOT_LOCK(est_pos);
-               pos.x = robot.est_pos.x;
-               pos.y = robot.est_pos.y;
-               pos.phi = robot.est_pos.phi;
+               ROBOT_LOCK(est_pos_uzv);
+               pos.x = robot.est_pos_uzv.x;
+               pos.y = robot.est_pos_uzv.y;
+               pos.phi = robot.est_pos_uzv.phi;
                pos.v = 0;
                pos.omega = 0;
-               ROBOT_UNLOCK(est_pos);
+               ROBOT_UNLOCK(est_pos_uzv);
        }
 }