robofsm: Just small positions and movement speed tuning.

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

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 9a9c92b2d2d7fa1ebf7c71e3f2f83540f3b92871..cbbe083d6a34dfcf9f3c1fe3124d827c02a1ae2a 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -34,8 +34,8 @@ 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),
+       robot_set_est_pos_trans(ROBOT_START_X_M,
+                               ROBOT_START_Y_M,
                                0);
 }
 
@@ -96,9 +96,9 @@ FSM_STATE(bypass_figure_in_front_of_start)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow);
+                       robot_trajectory_new(&tcFast);
                        robot_trajectory_add_point_trans(
-                               0.45 + 0.2,
+                               0.45 + 0.35,
                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
                        robot_trajectory_add_point_trans(
                                0.45 + 0.35 + 0.175,
@@ -130,7 +130,7 @@ FSM_STATE(approach_second_green_figure)
                        robot.use_right_bumper = true;
                        robot.use_back_bumpers = true;
 
-                       robot_trajectory_new(&tcSlow);
+                       robot_trajectory_new(&tcFast);
                        robot_trajectory_add_final_point_trans(
                                0.45 + 0.35,
                                0.29 + 0.28,
@@ -182,7 +182,7 @@ FSM_STATE(go_out_second_green_figure)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new_backward(&tcSlow);
+                       robot_trajectory_new_backward(&tcFast);
                        robot_trajectory_add_final_point_trans(0.45 + 0.35, 0.7, NO_TURN());
                        break;
                case EV_MOTION_DONE:
@@ -228,8 +228,8 @@ FSM_STATE(leave_protected_figure)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       //FSM_TIMER(1000);
-                       robot_trajectory_new_backward(&tcSlow);
+                       FSM_TIMER(2000);
+                       robot_trajectory_new_backward(&tcFast);
                        robot_trajectory_add_point_trans(
                                0.45 + 0.175,
                                0.7);
@@ -240,6 +240,9 @@ FSM_STATE(leave_protected_figure)
                        break;
                case EV_START:
                case EV_TIMER:
+                       act_jaws(CLOSE);
+                       //FSM_TRANSITION(load_second_figure);
+                       SUBFSM_RET(NULL);
                        break;
                case EV_RETURN:
                case EV_MOTION_DONE: