Six oranges somewhat works

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index cda92f5b42e62a9748f46fd73f15f3e95d348e8d..aab454b67c5ef4cce57882a51d3a454cdffa6018 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -166,13 +166,13 @@ FSM_STATE(climb_the_slope)
                case EV_ENTRY: {
                                // disables using side switches on bumpers when going up
                                enable_switches(false);
-                               act_vidle(VIDLE_LOAD_PREPARE, 5);
                                robot.ignore_hokuyo = true;
                                /* create the trajectory and go */
                                tc = tcSlow;
                                tc.maxacc = 0.4;
                                robot_trajectory_new_backward(&tc);
                                if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_BOUNDARY) {
+                                       act_vidle(VIDLE_LOAD_PREPARE, 5);
                                        robot_trajectory_add_point_trans(
                                                x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
                                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01);
@@ -181,6 +181,7 @@ FSM_STATE(climb_the_slope)
                                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
                                                NO_TURN());
                                } else if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_CENTER) {
+                                       FSM_TIMER(3000);
                                        robot_trajectory_add_point_trans(
                                                x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
                                                1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85));
@@ -198,6 +199,8 @@ FSM_STATE(climb_the_slope)
                        FSM_TRANSITION(sledge_down);
                        break;
                case EV_TIMER:
+                       act_vidle(VIDLE_LOAD_PREPARE, 5);
+                       break;
                case EV_START:
                case EV_VIDLE_DONE:
                case EV_MOTION_ERROR: