Merge branch 'master' of eurobot@michal:git

diff --combined src/robofsm/homologation.cc
index b852fe5f54de85a88190dbbcc0f53f2b2957a364,c13289f3b33076ef5f6237a488b5217af35edf94..13f24507ac1ed22e74471f30940c5503aaac9d99
--- 1/src/robofsm/homologation.cc
--- 2/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@@ -70,8 -70,6 +70,6 @@@ FSM_STATE(init
                        tcVerySlow.maxv = 0.05;
                        tcVerySlow.maxacc = 0.05;
                        
-                       act_vidle(VIDLE_UP);
-                       
                        FSM_TRANSITION(wait_for_start);
                        break;
                default:
@@@ -87,6 -85,11 +85,11 @@@ void set_initial_position(
                                DEG2RAD(180));
  }
  
+ void actuators_home()
+ {
+       act_vidle(VIDLE_UP);
+ }
+ 
  #ifdef COMPETITION
  #define WAIT_FOR_START
  #else
@@@ -116,6 -119,7 +119,7 @@@ FSM_STATE(wait_for_start
                case EV_START:
                        /* start competition timer */
                        sem_post(&robot.start);
+                       actuators_home();
                        set_initial_position();
                        FSM_TRANSITION(climb_the_slope);
                        break;
@@@ -126,6 -130,8 +130,8 @@@
                        // if the team color is changed during waiting
                        // for start.
                        set_initial_position();
+                       if (robot.start_state == START_PLUGGED_IN)
+                               actuators_home();
                        break;
                case EV_RETURN:
                case EV_GOAL_NOT_REACHABLE:
@@@ -221,14 -227,10 +227,14 @@@ FSM_STATE(to_container_diag
                        robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
                        break;
                case EV_MOTION_DONE:
 +                      FSM_TIMER(10000);
                        act_vidle(VIDLE_DOWN);
                        break;
 -              case EV_START:
                case EV_TIMER:
 +                      //act_vidle(VIDLE_UP);
 +                      //FSM_TRANSITION(experiment);
 +                      break;
 +              case EV_START:
                case EV_RETURN:
                case EV_ACTION_DONE:
                case EV_ACTION_ERROR:
@@@ -272,43 -274,26 +278,43 @@@ FSM_STATE(to_container_ortho
        }
  }
  
 +enum where_to_go {
 +      CORN,
 +      CONTAINER
 +};
 +
  static int cnt = 0;
  FSM_STATE(experiment)
  {
 +      static enum where_to_go where_to_go;
 +      static struct corn *corn_to_get;
        switch(FSM_EVENT) {
                case EV_ENTRY: {
                                double x, y, phi;
                                robot_get_est_pos(&x, &y, &phi);
 -                              printf("experiment: corn cnt: %d, est pos %.3f, %.3f, %.3f\n",
 +                              printf("experiment: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
                                        cnt, x, y, phi);
 -                              struct corn * corn = choose_next_corn();
 -                              Pos *p = get_corn_approach_position(corn /*&robot.corns->corns[cnt++]*/);
 -                              corn->was_collected = true;
 -                              //robot_trajectory_new(&tcFast);
 -                              //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
 -                              robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
 -                              delete(p);
 +                              corn_to_get = choose_next_corn();
 +                              if (corn_to_get) {
 +                                      Pos *p = get_corn_approach_position(corn_to_get);
 +                                      corn_to_get->was_collected = true;
 +                                      //robot_trajectory_new(&tcFast);
 +                                      //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
 +                                      robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
 +                                      delete(p);
 +                              }
 +                              where_to_go = CONTAINER;
                                break;
                        }
                case EV_MOTION_DONE:
 -                      FSM_TRANSITION(experiment);
 +                      cnt++;
 +                      if (where_to_go == CORN) {
 +                              FSM_TRANSITION(experiment);
 +                      } else {
 +                              remove_wall_around_corn(corn_to_get);
 +                              robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
 +                              where_to_go = CORN;
 +                      }
                        break;
                case EV_START:
                case EV_TIMER:
@@@ -322,6 -307,8 +328,6 @@@
        }
  }
  
 -
 -
  /************************************************************************
   * MAIN FUNCTION
   ************************************************************************/