From: petr <silhavik.p@gmail.com>
Date: Fri, 27 Apr 2012 13:45:37 +0000 (+0200)
Subject: robofsm: Odometry autocalibration
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/6399595c5da6be12913c5ee6d0a228e1c2bc07ba

robofsm: Odometry autocalibration

Prepare functions for odometry autocalibration.
---

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 77575a95..e49f2690 100644
--- 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 06110855..a0eee2e7 100644
--- 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 6489a5f5..10dad10a 100644
--- 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);