Odometry calibration

[eurobot/public.git] / src / robofsm / common-states.cc

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;