Odometry autocalibration

Remove calculation of distance using motor irc.

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 970e9eda8e1b0c953306f1c2b3f938af092ecd1b..b7d27b2d1002bc8ac92108b1c1ad127b1ee2cac8 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -435,12 +435,12 @@ FSM_STATE(go_back_for_cal)
                                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
                                                0);
                        robot_move_by(-1.1, NO_TURN(), &tcSlow);
-                       FSM_TIMER(500);
+                       FSM_TIMER(200);
                        break;
                case EV_MOTION_DONE:
                        ROBOT_LOCK(est_pos_odo);
-                       robot.odo_cal_a = 1/robot.odo_distance_a;
-                       robot.odo_cal_b = 1/robot.odo_distance_b;
+                       robot.odo_cal_a = -1.0/robot.odo_distance_a;
+                       robot.odo_cal_b = -1.0/robot.odo_distance_b;
                        x1 = robot.odo_distance_a;
                        y1 = robot.odo_distance_b;
                        ROBOT_UNLOCK(est_pos_odo);
@@ -463,7 +463,7 @@ FSM_STATE(go_back_for_cal)
                                FSM_SIGNAL(MOTION,EV_MOVE_STOP, NULL);
                                FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
                        } else {
-                               FSM_TIMER(500);
+                               FSM_TIMER(200);
                        }
                        ROBOT_UNLOCK(est_pos_odo);
                        break;

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 10dad10a3af0eada29188a7b5805d526947e349d..02475186dca0734520fedf7d865dd0c8d9ccbdc3 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -189,8 +189,6 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
                        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);