Odometry calibration

Correct measured distance.

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index f0b910d8dbb21057d8ffbc74a84792e9034fa2d3..d4a0c7b899851412ded400d6336984bb53f3ea79 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -35,9 +35,9 @@ UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file
 
 void set_initial_position()
 {
-       robot_set_est_pos_trans(ROBOT_START_X_M,
-                               ROBOT_START_Y_M,
-                               DEG2RAD(ROBOT_START_ANGLE_DEG));
+       robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
+                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0) - 0.05,
+                               0);
 }
 
 void actuators_home()
@@ -435,7 +435,7 @@ FSM_STATE(go_back_for_cal)
                                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
                                                0);
                        robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
-                       FSM_TIMER(200);
+                       FSM_TIMER(150);
                        break;
                case EV_MOTION_DONE:
                        ROBOT_LOCK(est_pos_odo);
@@ -454,6 +454,7 @@ FSM_STATE(go_back_for_cal)
                        fputs ("\n", file);
                        sprintf(buffer, "%4.4f", -1/y1);
                        fputs (buffer,file);
+                       fputs ("\n", file);
                        fclose(file);
                        SUBFSM_RET(NULL);
                        break;

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index e49f2690cd2d8c4cd5d55b4f5fe1a68b3923515c..5aeb5dd432bea727b7432757c731d90291a54d9d 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -31,6 +31,7 @@ UL_LOG_CUST(ulogd_robot); /* Log domain name = ulogd + name of the file */
 
 #define MOTION_CONTROL_INIT_ONLY
 #include "motion-control.h"
+#include "robot.h"
 
 /* Global definition of robot structure */
 struct robot robot;
@@ -317,4 +318,6 @@ void robot_calibrate_odometry()
        }
        robot.odo_cal_a = a / num;
        robot.odo_cal_b = b / num;
+       printf("calibrated value left: %f\n",robot.odo_cal_a);
+       printf("calibrated value right: %f\n",robot.odo_cal_b);
 }

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 02475186dca0734520fedf7d865dd0c8d9ccbdc3..9d5a231739f741eb309295b2f84c0373996e1650 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -182,8 +182,8 @@ 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  * robot.odo_cal_b;
-                       dleft = ((instance->right - robot.motion_irc.right) >> 8) * c  * robot.odo_cal_a;
+                       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);

diff --git a/src/robofsm/strategy_odo_calibration.cc b/src/robofsm/strategy_odo_calibration.cc
index fb13894a0527ae003c73122a7afd2fb19c9521e0..6c0327223908e439e75d79b348cd78504bd7ebfb 100644 (file)
--- a/src/robofsm/strategy_odo_calibration.cc
+++ b/src/robofsm/strategy_odo_calibration.cc
@@ -19,7 +19,7 @@ FSM_STATE(calibrate)
                        break;
                case EV_RETURN:
                        robot_calibrate_odometry();
-                       //FSM_TRANSITION(get_central_buillon_first);
+                       FSM_TRANSITION(get_central_buillon_first);
                        break;
                case EV_SWITCH_STRATEGY:
                        //FSM_TRANSITION(get_central_buillon_first);