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

diff --combined src/robofsm/common-states.cc
index 55efaa0107c379aa170260b4aa35c100489c38e7,3ac67653fdd3e2b84611cf370af191ab179ec097..db98c716fdb7fed8d2380c7a8f967705900855c0
--- 1/src/robofsm/common-states.cc
--- 2/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@@ -51,6 -51,7 +51,7 @@@ void start_entry(
        robot.check_turn_safety = false;
        pthread_create(&thid, NULL, timing_thread, NULL);
        start_timer();
+       act_camera_on();
  }
  
  // We set initial position periodically in order for it to be updated
@@@ -80,7 -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
  
@@@ -176,15 -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;
@@@ -311,17 -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;
@@@ -360,39 -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);