Merge branch 'master' of ssh://jaresf1@rtime.felk.cvut.cz/var/git/eurobot

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

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 3ac67653fdd3e2b84611cf370af191ab179ec097..db98c716fdb7fed8d2380c7a8f967705900855c0 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -81,7 +81,7 @@ void start_exit()
  * Trajectory constraints used; They are  initialized in the main() function in competition.cc
  ************************************************************************/
 
-struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
+struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow;
 
 #define VIDLE_TIMEOUT 2000
 
@@ -177,13 +177,15 @@ 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(3500);
+                                       FSM_TIMER(3800);
                                        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));
+                                               //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85));
+                                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199);
                                        robot_trajectory_add_final_point_trans(
                                                x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, slope_approach_style_p->which_side),
-                                               1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85),
+                                               //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85),
+                                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199,
                                                NO_TURN());
                                }
                                break;
@@ -310,15 +312,17 @@ FSM_STATE(sledge_down)
                                        PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
                                robot_trajectory_add_final_point_trans(
                                        x_coord(0.5 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
-                                       PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.17,
+                                       PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.19,
                                        NO_TURN());
                        } else if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_CENTER) {
                                robot_trajectory_add_point_trans(
                                        x_coord(1 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
-                                       1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85));
+                                       //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85));
+                                       PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199);
                                robot_trajectory_add_final_point_trans(
                                        x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.2, slope_approach_style_p->which_side),
-                                       1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85),
+                                       //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85),
+                                       PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199,
                                        NO_TURN());
                        }
                        break;
@@ -357,9 +361,39 @@ FSM_STATE(to_cntainer_and_unld)
                        robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcFast);
                        break;
                case EV_MOTION_DONE:
-                       FSM_TIMER(3000); // FIXME: test this
+                       FSM_TIMER(1000); // FIXME: test this
                        act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
                        break;
+               case EV_TIMER:
+                       FSM_TRANSITION(jerk);
+                       break;
+               case EV_START:
+               case EV_RETURN:
+               case EV_VIDLE_DONE:
+               case EV_MOTION_ERROR:
+               case EV_SWITCH_STRATEGY:
+                       DBG_PRINT_EVENT("unhandled event");
+               case EV_EXIT:
+                       break;
+       }
+}
+
+FSM_STATE(jerk)
+{
+       static char move_cnt;
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       move_cnt = 0;
+                       robot_move_by(-0.05, NO_TURN(), &tcSlow);
+                       break;
+               case EV_MOTION_DONE:
+                       if (move_cnt == 0) {
+                               robot_move_by(0.05, NO_TURN(), &tcJerk);
+                       } else if (move_cnt > 0) {
+                               FSM_TIMER(1500); // FIXME: test this
+                       }
+                       move_cnt++;
+                       break;
                case EV_TIMER:
                        act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
                        SUBFSM_RET(NULL);