+ ShmapFree();
+ ul_logdeb("robofsm: stop.\n");
+}
+
+void robot_get_est_pos_trans(double *x, double *y, double *phi)
+{
+ robot_get_est_pos(x, y, phi);
+ *x = __trans_x(*x);
+ *y = __trans_y(*y);
+ *phi = __trans_ang(*phi);
+}
+
+void robot_get_est_pos(double *x, double *y, double *phi)
+{
+ if (robot.indep_odometry_works) {
+ ROBOT_LOCK(est_pos_indep_odo);
+ *x = robot.est_pos_indep_odo.x;
+ *y = robot.est_pos_indep_odo.y;
+ *phi = robot.est_pos_indep_odo.phi;
+ ROBOT_UNLOCK(est_pos_indep_odo);
+ } else if (robot.odometry_works) {
+ ROBOT_LOCK(est_pos_odo);
+ *x = robot.est_pos_odo.x;
+ *y = robot.est_pos_odo.y;
+ *phi = robot.est_pos_odo.phi;
+ ROBOT_UNLOCK(est_pos_odo);
+ } else {
+ ROBOT_LOCK(ref_pos);
+ *x = robot.ref_pos.x;
+ *y = robot.ref_pos.y;
+ *phi = robot.ref_pos.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);