Odometry calibration

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

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 6489a5f5b14843050cba37c0f098666b255764ae..9d5a231739f741eb309295b2f84c0373996e1650 100644 (file)
--- 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);
@@ -182,7 +184,7 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
                        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;
-
+                       
                        dtang = (dleft + dright) / 2.0;
                        dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);