robofsm: Strategy

Implicitly wait for start plug.

Correction of odo calibration in robomon.

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 7556727b65ad86d97dbb28e77643c86d7d2f23df..1975f71a12168de57b250bb8ac81fa8f8e6c5528 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -36,7 +36,7 @@ UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file
 void set_initial_position()
 {
        robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
-                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0) - 0.05,
+                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
                                0);
 }
 
@@ -444,18 +444,20 @@ FSM_STATE(go_back_for_cal)
                        x1 = robot.odo_distance_a;
                        y1 = robot.odo_distance_b;
                        ROBOT_UNLOCK(est_pos_odo);
-                       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);
+                       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:

diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc
index 64d1e2186eade497da8a95315b18daa1478fd710..f9bb0a86a6537de1fd31d894717918b1be3d5a10 100644 (file)
--- a/src/robofsm/competition2012.cc
+++ b/src/robofsm/competition2012.cc
@@ -49,7 +49,7 @@ int main()
        robot.fsm.motion.debug_states = 1;
        //robot.fsm.act.debug_states = 1;
 
-       robot.fsm.main.state = &fsm_state_main_calibrate;
+       robot.fsm.main.state = &fsm_state_main_get_central_buillon_first;
        //robot.fsm.main.transition_callback = trans_callback;
        //robot.fsm.motion.transition_callback = move_trans_callback;
 

diff --git a/src/robofsm/strategy_odo_calibration.cc b/src/robofsm/strategy_odo_calibration.cc
index 6c0327223908e439e75d79b348cd78504bd7ebfb..0ce52f657398a8eac93221a7dc6d457708ee477a 100644 (file)
--- a/src/robofsm/strategy_odo_calibration.cc
+++ b/src/robofsm/strategy_odo_calibration.cc
@@ -19,10 +19,10 @@ 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);
+                       FSM_TRANSITION(get_central_buillon_first);
                        break;
                default:;
        }