robofsm: Move odometry calibration function to special file.

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 7704096c42e8b6fce6c05603759a8b7efc4fd5d8..f80d263d9f0fb3ca241391f9570ff13f82061030 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -288,38 +288,3 @@ 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, "ODO calibration 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("ODO calibrated value A: %f\n",robot.odo_cal_a);
-       printf("ODO calibrated value B: %f\n",robot.odo_cal_b);
-}

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 8e0714486c3ab2be9a2b8e9225d6e8b3d46a00f1..cd5f328b4f694c0a15c72a55a58fd96369f1d269 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -231,8 +231,6 @@ 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/strategy_odo_calibration.cc b/src/robofsm/strategy_odo_calibration.cc
index 877afae86796cee3ca3e5bdf9296d02742df1116..b78750016ab99ae56e4a9f092a36516defaf8a58 100644 (file)
--- a/src/robofsm/strategy_odo_calibration.cc
+++ b/src/robofsm/strategy_odo_calibration.cc
@@ -19,7 +19,41 @@
 #include <ul_log.h>
 #include "common-states.h"
 
-
+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, "ODO calibration 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 ++;
+                printf("a: %i, b: %i, num: %i\n", a, b, 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("ODO calibrated value A: %f\n",robot.odo_cal_a);
+        printf("ODO calibrated value B: %f\n",robot.odo_cal_b);
+}
 
 UL_LOG_CUST(ulogd_strategy_odo_calibration); /* Log domain name = ulogd + name of the file */