+
+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;
+}