homologation: improve the experiment with corn rushing

- use several states
- rename the experiment state to approach_next_corn
- the "central corn rushing state" is called "experiment decider"

diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc
index 13f24507ac1ed22e74471f30940c5503aaac9d99..7a62676852ea050348f6194f4b884f44fca915f8 100644 (file)
--- a/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@ -49,7 +49,10 @@ FSM_STATE_DECL(climb_the_slope);
 FSM_STATE_DECL(sledge_down);
 FSM_STATE_DECL(to_container_diag);
 FSM_STATE_DECL(to_container_ortho);
-FSM_STATE_DECL(experiment);
+FSM_STATE_DECL(experiment_decider);
+FSM_STATE_DECL(approach_next_corn);
+FSM_STATE_DECL(rush_the_corn);
+FSM_STATE_DECL(turn_around);
 
 /************************************************************************
  * INITIAL AND STARTING STATES
@@ -227,12 +230,12 @@ 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);
+                       FSM_TIMER(6000);
                        act_vidle(VIDLE_DOWN);
                        break;
                case EV_TIMER:
-                       //act_vidle(VIDLE_UP);
-                       //FSM_TRANSITION(experiment);
+                       act_vidle(VIDLE_UP);
+                       FSM_TRANSITION(approach_next_corn);
                        break;
                case EV_START:
                case EV_RETURN:
@@ -278,22 +281,52 @@ FSM_STATE(to_container_ortho)
        }
 }
 
-enum where_to_go {
+static enum where_to_go {
        CORN,
-       CONTAINER
-};
+       TURN_AROUND,
+       CONTAINER,
+       NO_MORE_CORN
+} where_to_go = CORN;
+
+static struct corn *corn_to_get;
+
+FSM_STATE(experiment_decider)
+{
+       
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       if (where_to_go == CORN) {
+                               FSM_TRANSITION(approach_next_corn);
+                       } else if (where_to_go == CONTAINER) {
+                               FSM_TRANSITION(rush_the_corn);
+                       } else if (where_to_go == TURN_AROUND) {
+                               FSM_TRANSITION(turn_around);
+                       } else /* NO_MORE_CORN */ { 
+                       }
+                       break;
+               case EV_START:
+               case EV_TIMER:
+               case EV_RETURN:
+               case EV_ACTION_DONE:
+               case EV_ACTION_ERROR:
+               case EV_MOTION_DONE:
+               case EV_GOAL_NOT_REACHABLE:
+                       DBG_PRINT_EVENT("unhandled event");
+               case EV_EXIT:
+                       break;
+       }
+}
 
 static int cnt = 0;
-FSM_STATE(experiment)
+FSM_STATE(approach_next_corn)
 {
-       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: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
+                               printf("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
                                        cnt, x, y, phi);
+
                                corn_to_get = choose_next_corn();
                                if (corn_to_get) {
                                        Pos *p = get_corn_approach_position(corn_to_get);
@@ -302,19 +335,15 @@ FSM_STATE(experiment)
                                        //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;
+                               } else {
+                                       where_to_go = NO_MORE_CORN;
                                }
-                               where_to_go = CONTAINER;
                                break;
                        }
                case EV_MOTION_DONE:
                        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;
-                       }
+                       FSM_TRANSITION(experiment_decider);
                        break;
                case EV_START:
                case EV_TIMER:
@@ -328,6 +357,54 @@ FSM_STATE(experiment)
        }
 }
 
+FSM_STATE(rush_the_corn)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       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 = TURN_AROUND;
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(experiment_decider);
+                       break;
+               case EV_START:
+               case EV_TIMER:
+               case EV_RETURN:
+               case EV_ACTION_DONE:
+               case EV_ACTION_ERROR:
+               case EV_GOAL_NOT_REACHABLE:
+                       DBG_PRINT_EVENT("unhandled event");
+               case EV_EXIT:
+                       break;
+       }
+}
+
+// used to perform the maneuvre
+FSM_STATE(turn_around)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new_backward(&tcFast);
+                       robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90));
+                       break;
+               case EV_MOTION_DONE:
+                       where_to_go = CORN;
+                       FSM_TRANSITION(experiment_decider);
+                       break;
+               case EV_START:
+               case EV_TIMER:
+               case EV_RETURN:
+               case EV_ACTION_DONE:
+               case EV_ACTION_ERROR:
+               case EV_GOAL_NOT_REACHABLE:
+                       DBG_PRINT_EVENT("unhandled event");
+               case EV_EXIT:
+                       break;
+       }
+}
+
+
 /************************************************************************
  * MAIN FUNCTION
  ************************************************************************/