robofsm: Odometry autocalibration

Prepare functions for odometry autocalibration.

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 77575a95d6d8e522cbfb26f7eb7218f35ddee99a..e49f2690cd2d8c4cd5d55b4f5fe1a68b3923515c 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -104,7 +104,7 @@ int robot_init()
 {
        int rv;
        pthread_mutexattr_t mattr;
-
+       robot_calibrate_odometry();
        rv = pthread_mutexattr_init(&mattr);
 #ifdef HAVE_PRIO_INHERIT
        rv = pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
@@ -285,3 +285,36 @@ void robot_get_est_pos(double *x, double *y, double *phi)
                ROBOT_UNLOCK(ref_pos);
        }
 }
+
+void robot_calibrate_odometry()
+{
+       robot.odo_distance_a = 0;
+       robot.odo_distance_b = 0;
+       FILE *file;     
+       file = fopen ("odometry_cal_data", "r");
+       if (file == NULL)
+       { 
+               robot.odo_cal_a = 1;
+               robot.odo_cal_b = 1;
+               fprintf(stderr, "File not found. \n\n");
+               return;
+       }
+       char line [10];
+       float a = 0;
+       float b = 0;
+       int num = 0;
+       while (fgets (line, 10 , file)) {
+               a = atof(line);
+               fgets (line, 10 , file);
+               b = atof(line);
+               num ++;
+       }
+       fclose (file);
+       if(a == 0 || b == 0) {
+               robot.odo_cal_a = 1;
+               robot.odo_cal_b = 1;
+               return;
+       }
+       robot.odo_cal_a = a / num;
+       robot.odo_cal_b = b / num;
+}

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 06110855e92d11ada2f2add7028c0655ffcd398e..a0eee2e76ba6263edc7ece43b76c29d0ff0cce1b 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -214,6 +214,11 @@ struct robot {
        /** is set to true in separate thread when there is short time left */
        bool short_time_to_end;
        bool check_turn_safety;
+       
+       float odo_cal_b;
+       float odo_cal_a;
+       float odo_distance_a;
+       float odo_distance_b;
 }; /* robot */
 
 extern struct robot robot;
@@ -226,6 +231,7 @@ int robot_init() __attribute__ ((warn_unused_result));
 int robot_start() __attribute__ ((warn_unused_result));
 void robot_exit();
 void robot_destroy();
+void robot_calibrate_odometry();
 
 void robot_get_est_pos_trans(double *x, double *y, double *phi);
 void robot_get_est_pos(double *x, double *y, double *phi);

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 6489a5f5b14843050cba37c0f098666b255764ae..10dad10a3af0eada29188a7b5805d526947e349d 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -123,13 +123,15 @@ void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
                                break;
                        }
                        
-                       dleft = ((robot.odo_data.left - instance->left) >> 8) * c;      // TODO >> 8 ?
-                       dright = ((instance->right - robot.odo_data.right) >> 8) * c;
+                       dleft = ((robot.odo_data.left - instance->left) >> 8) * c * robot.odo_cal_a;    // TODO >> 8 ?
+                       dright = ((instance->right - robot.odo_data.right) >> 8) * c * robot.odo_cal_b;
 
                        dtang = (dleft + dright) / 2.0;
                        dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
 
                        ROBOT_LOCK(est_pos_indep_odo);
+                       robot.odo_distance_a +=dleft;
+                       robot.odo_distance_b +=dright;
                        double a = robot.est_pos_indep_odo.phi;
                        robot.est_pos_indep_odo.x += dtang*cos(a);
                        robot.est_pos_indep_odo.y += dtang*sin(a);
@@ -180,13 +182,15 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
                        /* FIXME maybe it is not correct to do this nasty hack here (switch dleft and dright),
                        what is the right solution?
                        This was neccessary to view motor odometry correctly in robomon. */
-                       dright = ((robot.motion_irc.left - instance->left) >> 8) * c;
-                       dleft = ((instance->right - robot.motion_irc.right) >> 8) * c;
-
+                       dright = ((robot.motion_irc.left - instance->left) >> 8) * c  * robot.odo_cal_b;
+                       dleft = ((instance->right - robot.motion_irc.right) >> 8) * c  * robot.odo_cal_a;
+                       
                        dtang = (dleft + dright) / 2.0;
                        dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
 
                        ROBOT_LOCK(est_pos_odo);
+                       robot.odo_distance_a +=dleft;
+                       robot.odo_distance_b +=dright;
                        double a = robot.est_pos_odo.phi;
                        robot.est_pos_odo.x += dtang*cos(a);
                        robot.est_pos_odo.y += dtang*sin(a);