Independent odometry tuning

we are able to run rectangle 1m/s and after 90sec. position errro was 10cm

diff --git a/src/robodim/robodim.h b/src/robodim/robodim.h
index 27bb1a3df8bd81f37e7fb1a1fa528828e821a174..8cbb9a1acb927e3c0f37ecf818c922fdb5ee5e55 100644 (file)
--- a/src/robodim/robodim.h
+++ b/src/robodim/robodim.h
@@ -53,7 +53,7 @@
 #define HOKUYO_CENTER_OFFSET_M (HOKUYO_CENTER_OFFSET_MM/1000.0)
 #define ODOMETRY_WHEEL_RADIUS_MM 30
 #define ODOMETRY_WHEEL_RADIUS_M (ODOMETRY_WHEEL_RADIUS_MM/1000.0)
-#define ODOMETRY_ROTATION_RADIUS_MM (245/2)
+#define ODOMETRY_ROTATION_RADIUS_MM (246/2)
 #define ODOMETRY_ROTATION_RADIUS_M (ODOMETRY_ROTATION_RADIUS_MM/1000.0)
 
 /**

diff --git a/src/robofsm/movehelper.cc b/src/robofsm/movehelper.cc
index 57448f216795492099cb255c3b7592496fc057ad..cea4aed008291b38b9797c15ccdbd10a578e2702 100644 (file)
--- a/src/robofsm/movehelper.cc
+++ b/src/robofsm/movehelper.cc
@@ -84,11 +84,12 @@ void robot_set_est_pos_notrans(double x, double y, double phi)
        if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M;
        if (y<0) y=0;
        if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M;
-       ROBOT_LOCK(est_pos_uzv);
-       robot.est_pos_uzv.x = x;
-       robot.est_pos_uzv.y = y;
-       robot.est_pos_uzv.phi = phi;
-       init_ekf_flag = true;   // Tell kalman filter to reinitialize itself
+
+       ROBOT_LOCK(est_pos_indep_odo);
+       robot.est_pos_indep_odo.x = x;
+       robot.est_pos_indep_odo.y = y;
+       robot.est_pos_indep_odo.phi = phi;
+       ROBOT_UNLOCK(est_pos_indep_odo);
 
        ROBOT_LOCK(est_pos_odo);
        robot.est_pos_odo.x = x;
@@ -101,9 +102,6 @@ void robot_set_est_pos_notrans(double x, double y, double phi)
        robot.ref_pos.y = y;
        robot.ref_pos.phi = phi;
        ROBOT_UNLOCK(ref_pos);
-
-       ROBOT_UNLOCK(est_pos_uzv);
-
 }
 
 /** 

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 46788de75af02ea08fe5a03dccf57ba91882e62d..902eded9eb00fa9a5d43e0e20e1e9fc5c4ce10d4 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -110,7 +110,7 @@ int robot_init()
        pthread_mutex_init(&robot.lock_ref_pos, &mattr);
        pthread_mutex_init(&robot.lock_est_pos_uzv, &mattr);
        pthread_mutex_init(&robot.lock_est_pos_odo, &mattr);
-       pthread_mutex_init(&robot.lock_est_pos_indep_odo, &mattr);      // FIXME add MV
+       pthread_mutex_init(&robot.lock_est_pos_indep_odo, &mattr);
        pthread_mutex_init(&robot.lock_meas_angles, &mattr);
        pthread_mutex_init(&robot.lock_joy_data, &mattr);
        pthread_mutex_init(&robot.lock_disp, &mattr);

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 0eba89bf7ba8374c772aa1db1ef2c52b5d213c86..aa7aaeac41531686342382e0ead1d19a012276ce 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -114,7 +114,7 @@ void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
                        dright = ((instance->right - robot.odo_data.right) >> 8) * c;
 
                        dtang = (dleft + dright) / 2.0;
-                       dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
+                       dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
 
                        ROBOT_LOCK(est_pos_indep_odo);
                        double a = robot.est_pos_indep_odo.phi;

diff --git a/src/robofsm/test/rectangle.cc b/src/robofsm/test/rectangle.cc
index b1c8fab7c89242cbe7998863f19476f6e1126a57..8ec14f2048089f599fa8396ec09b751665ab4a84 100644 (file)
--- a/src/robofsm/test/rectangle.cc
+++ b/src/robofsm/test/rectangle.cc
@@ -24,7 +24,7 @@ FSM_STATE(init) {
        /* Where we are at the begining? */
        switch (FSM_EVENT) {
                case EV_ENTRY:
-                       FSM_TIMER(2000);
+                       FSM_TIMER(10);
                        break;
                case EV_TIMER:
                        FSM_TRANSITION(rectangle);
@@ -43,14 +43,15 @@ FSM_STATE(init) {
 void follow_rectangle()
 {
        double rectangle[][2] = { { 0.5, 0.5 },
-                                 { 1.2, 0.5 },
-                                 { 1.2, 1.2 },
-                                 { 0.5, 1.2 }};
+                                 { 1.5, 0.5 },
+                                 { 1.5, 1.5 },
+                                 { 0.5, 1.5 }};
        static bool backward = false;
        struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
-       tc.maxv *= 0.2;
-       tc.maxacc *= 0.2;
-       tc.maxomega *= 0.2;
+       tc.maxv = 1.0;
+       tc.maxacc = 0.5;
+       tc.maxomega = 2;
+       tc.maxe = 0.200;
 
        /* Allocate new trajectory */
        if (!backward)
@@ -65,7 +66,7 @@ void follow_rectangle()
                if (i<3) {
                        robot_trajectory_add_point_trans(rectangle[j][0], rectangle[j][1]);
                } else {
-                       robot_trajectory_add_final_point_trans(rectangle[j][0], rectangle[j][1], NO_TURN());
+                       robot_trajectory_add_final_point_trans(rectangle[j][0], rectangle[j][1], TURN(0));
                }
        }
        //backward = !backward;
@@ -78,7 +79,7 @@ FSM_STATE(rectangle)
                        follow_rectangle();
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(turn);
+                       //FSM_TRANSITION(turn);
                        FSM_TRANSITION(wait);
                        break;
                default: