Odometry autocalibration

[eurobot/public.git] / src / robofsm / robot.c

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 77575a95d6d8e522cbfb26f7eb7218f35ddee99a..57e690a8b1701e9890f5addb90254d41fc566c8d 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -31,6 +31,7 @@ UL_LOG_CUST(ulogd_robot); /* Log domain name = ulogd + name of the file */
 
 #define MOTION_CONTROL_INIT_ONLY
 #include "motion-control.h"
+#include "robot.h"
 
 /* Global definition of robot structure */
 struct robot robot;
@@ -104,7 +105,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 +286,38 @@ 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 [15];
+       float a = 0;
+       float b = 0;
+       int num = 0;
+       while (fgets (line, 15 , file)) {
+               a += atof(strtok(line," "));
+               fgets (line, 15 , file);
+               b += atof(strtok(NULL," "));
+               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;
+       printf("calibrated value left: %f\n",robot.odo_cal_a);
+       printf("calibrated value right: %f\n",robot.odo_cal_b);
+}