warnings in homologation.cc avoided

MAIN FSM:
    - look around for puck action commented out, updating the code to take
      into account that there is no sharp sensor present now

EV_PREPARE_FOR_LOAD action removed

diff --git a/src/robofsm/fsmmain.c b/src/robofsm/fsmmain.c
index 62efebb46d453bc9938725b25a8053e266feab15..2a047a65f41f7ade935e7c88e12ff81e299c8116 100644 (file)
--- a/src/robofsm/fsmmain.c
+++ b/src/robofsm/fsmmain.c
@@ -48,8 +48,8 @@
 typedef enum {
        LOAD_SUCCESS = 0,
        LOAD_FAIL,
-       LOOK_AROUND_SUCCESS,
-       LOOK_AROUND_FAIL
+       //LOOK_AROUND_SUCCESS,  // obsolete, no sharp sensor
+       //LOOK_AROUND_FAIL      // obsolete, no sharp sensor
 } SUBFSM_RET_VAL;
 
 #define FSM_EVENT_RET_VAL ((SUBFSM_RET_VAL)FSM_EVENT_PTR)
@@ -61,10 +61,10 @@ typedef enum {
 struct TrajectoryConstraints tcFast, tcSlow;
 
 /************************************************************************
- * Trajectory constraints used, are initialized in the init state
+ * Variables related to puck collecting
  ************************************************************************/
 
-int free_puck_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 = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
 
 /************************************************************************
  * Definition of particular "free pucks pick up sequences"
@@ -358,53 +358,39 @@ static void robot_goto_next_puck_in_sequence(int lot, int puck_number)
 FSM_STATE(collect_free_pucks)
 {
        static const int lot = 7;  // this variable location is temporary...; going to be received from the camera
-       static int puck_approach_attempt_no;
        switch (FSM_EVENT) {
                case EV_ENTRY:
-                       puck_approach_attempt_no = 0;
-                       FSM_SIGNAL(ACT, EV_PREPARE_FOR_LOAD, NULL);
-                       robot_goto_next_puck_in_sequence(lot, free_puck_to_get_next);
+                       robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
                        break;
                case EV_MOTION_DONE: {
-                               printf("-----arrived where the free puck no. %d should be\n", free_puck_to_get_next);
-                               if (PUCK_REACHABLE(robot.puck_distance)) { 
-                                       SUBFSM_TRANSITION(load_the_puck, NULL);
-                               } else {
-                                       switch (puck_approach_attempt_no) {
-                                       case 0:
-                                               printf("-----moving forward by 5 cm\n");
-                                               robot_move_by(0.05, NO_TURN(), &tcSlow);
-                                               puck_approach_attempt_no++;
-                                               break;
-                                       case 1:
-                                               SUBFSM_TRANSITION(look_around_for_puck, NULL);
-                                               break;
-                                       }
-                               }
+                               printf("-----arrived where the free puck no. %d should be\n", free_puck_to_try_to_get_next);
+                               SUBFSM_TRANSITION(load_the_puck, NULL);
                        }
                        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;
+                               break; */
                        case LOAD_SUCCESS:
                                printf(">>>>>> Loading the puck succeeded\n");
-                               if(free_puck_to_get_next<4) {
-                                       free_puck_to_get_next++;
-                                       robot_goto_next_puck_in_sequence(lot, free_puck_to_get_next);
+                               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);
                                }
-                               puck_approach_attempt_no = 0;
                                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_get_next<6) { // FIXME: test number of pucks loaded!!
-                                       free_puck_to_get_next++;
-                                       robot_goto_next_puck_in_sequence(lot, free_puck_to_get_next);
+                               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
                                }
@@ -413,7 +399,7 @@ FSM_STATE(collect_free_pucks)
                        break;
                case EV_PUCK_REACHABLE:
                        robot_stop();
-                       printf("-----reached some free puck, hopefully no. %d\n", free_puck_to_get_next);
+                       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:
@@ -547,8 +533,10 @@ FSM_STATE(approach_opponents_static_dispenser)
 
 FSM_STATE(load_the_puck)
 {
+       static int puck_load_attempt_count;
        switch (FSM_EVENT) {
                case EV_ENTRY:
+                       puck_load_attempt_count = 0;
                        FSM_SIGNAL(ACT, EV_SCRABBLE, NULL);
                        FSM_TIMER(200);
                        break;
@@ -562,7 +550,12 @@ FSM_STATE(load_the_puck)
                        SUBFSM_RET((void *)LOAD_SUCCESS);
                        break;
                case EV_ACTION_ERROR:
-                       SUBFSM_RET((void *)LOAD_FAIL);
+                       puck_load_attempt_count++;
+                       if (puck_load_attempt_count > 2) {
+                               SUBFSM_RET((void *)LOAD_FAIL);
+                       } else {
+                               robot_move_by(0.02, NO_TURN(), &tcSlow);
+                       }
                        break;
                case EV_RETURN:
                case EV_LASER_POWER:
@@ -578,6 +571,7 @@ FSM_STATE(load_the_puck)
        }
 }
 
+/* of no use without the sharp sensor measuring "puck distance"
 FSM_STATE(look_around_for_puck)
 {
        static int lfp_status = 0;
@@ -630,6 +624,7 @@ FSM_STATE(look_around_for_puck)
                        break;
        }
 }
+*/
 
 
 /*

diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc
index e86385eff2add3583e20d8445237346b97432ff9..09c324b014b44302d031651e0d16bc0fd1958f0c 100644 (file)
--- a/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@ -72,6 +72,8 @@ FSM_STATE(wait_for_start)
        #endif
        switch (FSM_EVENT) {
 #ifdef WAIT_FOR_START
+               case EV_ENTRY:
+                       break
                case EV_START: {
                        pthread_t thid;
 
@@ -80,6 +82,7 @@ FSM_STATE(wait_for_start)
                        pthread_create(&thid, NULL, wait_to_deposition, NULL);
                        }
 #else
+               case EV_START:
                case EV_ENTRY:
 #endif
                        robot_set_est_pos_trans(0.16,

diff --git a/src/robofsm/roboevent.py b/src/robofsm/roboevent.py
index 20ba59afdef32581d4d8d7a53f23ca31a15d78c4..8d1852dffdb2f8354c4216e7f78e2ab77117fb0c 100644 (file)
--- a/src/robofsm/roboevent.py
+++ b/src/robofsm/roboevent.py
@@ -37,7 +37,6 @@ events = {
     "disp" : {
     },
     "act" : {
-       "EV_PREPARE_FOR_LOAD"   : "Signal from the main FSM to set lift to position suited for sharp measurement",
        "EV_SCRABBLE"           : "Signal from the main FSM to set the chelae to not so opened position",
         "EV_LOAD_THE_PUCK"     : "Signal from the main FSM to initiate the puck picking up procedure",