Merge branch 'master' of jaresf1@rtime:/var/git/eurobot

diff --git a/src/robofsm/competition.cc b/src/robofsm/competition.cc
index a7c02017609643cbf7c5fad8f49120aaae43f7b5..f5137fc2246a9ac5ca630a7474bdc4eb0505f42c 100644 (file)
--- a/src/robofsm/competition.cc
+++ b/src/robofsm/competition.cc
@@ -49,11 +49,9 @@
 typedef enum {
        LOAD_SUCCESS = 0,
        LOAD_FAIL,
-       //LOOK_AROUND_SUCCESS,  // obsolete, no sharp sensor
-       //LOOK_AROUND_FAIL      // obsolete, no sharp sensor
-} SUBFSM_RET_VAL;
+} subfsm_ret_val;
 
-#define FSM_EVENT_RET_VAL ((SUBFSM_RET_VAL)FSM_EVENT_INT)
+#define FSM_EVENT_RET_VAL ((subfsm_ret_val)FSM_EVENT_INT)
 
 /************************************************************************
  * Trajectory constraints used, are initialized in the init state
@@ -65,7 +63,9 @@ struct TrajectoryConstraints tcFast, tcSlow;
  * Variables related to puck collecting
  ************************************************************************/
 
-int free_puck_to_try_to_get_next = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
+int free_puck_to_try_to_get_next; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
+int pucks_at_once; // number of pucks to load at once (maximum number of pucks we want to have in robot)
+
 
 /************************************************************************
  * Definition of particular "free pucks pick up sequences"
@@ -262,7 +262,9 @@ void robot_goto_puck_in_grid(int nx, int ny, int phi)
 FSM_STATE_DECL(init);
 FSM_STATE_DECL(wait_for_start);
 /* strategies related states */
+FSM_STATE_DECL(decide_what_next);
 FSM_STATE_DECL(collect_free_pucks);
+FSM_STATE_DECL(deposit_at_acropolis);
 /* movement states */
 FSM_STATE_DECL(simple_construction_zone_approach);
 FSM_STATE_DECL(approach_our_static_dispenser);
@@ -281,6 +283,8 @@ FSM_STATE(init)
 {
        switch (FSM_EVENT) {
        case EV_ENTRY:
+               free_puck_to_try_to_get_next = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
+               pucks_at_once = 2; // number of pucks to load at once (maximum number of pucks we want to have in robot)
                tcFast = trajectoryConstraintsDefault;
                //tcFast.maxv = 1.5;
                tcSlow = trajectoryConstraintsDefault;
@@ -354,6 +358,59 @@ FSM_STATE(wait_for_start)
  * STRATEGIES RELATED STATES
  ************************************************************************/
 
+FSM_STATE(decide_what_next)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       if(robot.pucks_inside == pucks_at_once) {
+                               ; //
+                       } else {
+                               if(robot.pucks_inside < pucks_at_once && free_puck_to_try_to_get_next < 6) {
+                                       FSM_TRANSITION(collect_free_pucks);
+                               }
+                       }
+                       break;
+               case EV_MOTION_DONE:
+               case EV_ACTION_DONE:
+               case EV_RETURN:
+               case EV_TIMER:
+               case EV_LASER_POWER:
+               case EV_GOAL_NOT_REACHABLE:
+               case EV_SHORT_TIME_TO_END:
+               case EV_STACK_FULL:
+               case EV_ACTION_ERROR:
+               case EV_PUCK_REACHABLE:
+               case EV_START:
+                       DBG_PRINT_EVENT("unhandled event");
+                       break;
+               case EV_EXIT:
+                       break;
+       }
+}
+
+FSM_STATE(deposit_at_acropolis)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       break;
+               case EV_MOTION_DONE:
+               case EV_ACTION_DONE:
+               case EV_RETURN:
+               case EV_TIMER:
+               case EV_LASER_POWER:
+               case EV_GOAL_NOT_REACHABLE:
+               case EV_SHORT_TIME_TO_END:
+               case EV_STACK_FULL:
+               case EV_ACTION_ERROR:
+               case EV_PUCK_REACHABLE:
+               case EV_START:
+                       DBG_PRINT_EVENT("unhandled event");
+                       break;
+               case EV_EXIT:
+                       break;
+       }
+}
+
 static void robot_goto_next_puck_in_sequence(int lot, int puck_number)
 {
        robot_goto_puck_in_grid(
@@ -376,39 +433,16 @@ FSM_STATE(collect_free_pucks)
                        break;
                case EV_RETURN:
                        switch(FSM_EVENT_RET_VAL) {
-                       /* obsolete, no sharp sensor present
-                       case LOOK_AROUND_SUCCESS: // FIXME: test number of pucks loaded!!
-                               printf(">>>>>> Look around succeeded\n");
-                               SUBFSM_TRANSITION(load_the_puck, NULL);
-                               break; */
                        case LOAD_SUCCESS:
+                               free_puck_to_try_to_get_next++;
                                printf(">>>>>> Loading the puck succeeded\n");
-                               if(free_puck_to_try_to_get_next<4) {
-                                       free_puck_to_try_to_get_next++;
-                                       robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
-                               }
                                break;
-                       /* obsolete, no sharp sensor present
-                       case LOOK_AROUND_FAIL:
-                               printf(">>>>>> Looking around for the puck FAILED\n");
-                               break; // FIXME: remove the break
-                       */
                        case LOAD_FAIL:
                                printf(">>>>>> Loading the puck FAILED\n");
-                               if(free_puck_to_try_to_get_next<6) { // FIXME: test number of pucks loaded!!
-                                       free_puck_to_try_to_get_next++;
-                                       robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
-                               } else { 
-                                       // FIXME (TODO): transition to next strategy state
-                               }
                                break;
                        }
                        break;
                case EV_PUCK_REACHABLE:
-                       robot_stop();
-                       printf("-----reached some free puck, hopefully no. %d\n", free_puck_to_try_to_get_next);
-                       SUBFSM_TRANSITION(load_the_puck, NULL);
-                       break;
                case EV_ACTION_DONE:
                case EV_TIMER:
                case EV_LASER_POWER:
@@ -644,11 +678,9 @@ FSM_STATE()
                case EV_ACTION_DONE:
                case EV_RETURN:
                case EV_TIMER:
-               case EV_OBSTRUCTION_AHEAD:
                case EV_LASER_POWER:
                case EV_GOAL_NOT_REACHABLE:
                case EV_SHORT_TIME_TO_END:
-               case EV_ENEMY_AHEAD:
                case EV_STACK_FULL:
                case EV_ACTION_ERROR:
                case EV_PUCK_REACHABLE: