robofsm: Strategy

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

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 7fd646d5c7b510dcebc6952ae8a11f4144487e96..1975f71a12168de57b250bb8ac81fa8f8e6c5528 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);
 }
 
 void actuators_home()
@@ -434,25 +434,30 @@ 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.5, NO_TURN(), &tcSlow);
-                       FSM_TIMER(500);
+                       robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
+                       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);
-                       printf("EROOR%f", x1);
-                       FILE * file;
-                       file = fopen ("odometry_cal_data","a+");
-                       sprintf(buffer, "%4.4f", 1/x1);
-                       fputs (buffer,file);
-                       fputs ("\n", file);
-                       sprintf(buffer, "%4.4f", 1/y1);
-                       fputs (buffer,file);
-                       fclose(file);
+                       if(x1 != 0 || y1 != 0) {
+                               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);
+                               fputs (buffer,file);
+                               fputs (" ", file);
+                               sprintf(buffer, "%4.4f", -1/y1);
+                               fputs (buffer,file);
+                               fputs ("\n", file);
+                               fclose(file);
+                       } 
                        SUBFSM_RET(NULL);
                        break;
                case EV_TIMER:
@@ -463,7 +468,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;