Odometry autocalibration

Change sign for writing into file.

Update output for user. (show traveled distance)

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index b7d27b2d1002bc8ac92108b1c1ad127b1ee2cac8..f0b910d8dbb21057d8ffbc74a84792e9034fa2d3 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -434,7 +434,7 @@ FSM_STATE(go_back_for_cal)
                        robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M + 1.0,
                                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
                                                0);
-                       robot_move_by(-1.1, NO_TURN(), &tcSlow);
+                       robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
                        FSM_TIMER(200);
                        break;
                case EV_MOTION_DONE:
@@ -444,13 +444,15 @@ FSM_STATE(go_back_for_cal)
                        x1 = robot.odo_distance_a;
                        y1 = robot.odo_distance_b;
                        ROBOT_UNLOCK(est_pos_odo);
-                       printf("EROOR%f", x1);
+                       printf("Distance for calibration: \n");
+                       printf("Left%f\n", x1);
+                       printf("Right%f\n", y1);
                        FILE * file;
                        file = fopen ("odometry_cal_data","a+");
-                       sprintf(buffer, "%4.4f", 1/x1);
+                       sprintf(buffer, "%4.4f", -1/x1);
                        fputs (buffer,file);
                        fputs ("\n", file);
-                       sprintf(buffer, "%4.4f", 1/y1);
+                       sprintf(buffer, "%4.4f", -1/y1);
                        fputs (buffer,file);
                        fclose(file);
                        SUBFSM_RET(NULL);