robofsm: Try mac 3 times approach target.

If five times obstacle event occurs, send motion error signal.

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index 64b0710b647ab7a95f2fed9d5d6ff2651c77802f..587d98342db517f95c5746ad74d3b580176b0bdc 100644 (file)
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -302,6 +302,8 @@ FSM_STATE(wait_for_command)
 
 FSM_STATE(movement)
 {
+        static int obstacle_cntr = 0;
+        
        switch (FSM_EVENT) {
                case EV_ENTRY:
                        break;
@@ -315,6 +317,11 @@ FSM_STATE(movement)
                        break;
                case EV_OBSTACLE:
                         stop();
+                        if (obstacle_cntr++ > 5) {
+                                FSM_SIGNAL(MAIN, EV_MOTION_ERROR, NULL);
+                                FSM_TRANSITION(wait_for_command);
+                                break;
+                        }
                        switch (recalculate_trajectory()) {
                                // We can go to the target:
                                case TARGET_OK:    break;
@@ -438,12 +445,12 @@ FSM_STATE(wait_and_try_again)
                                case TARGET_OK:    FSM_TRANSITION(movement); break;
                                // Target inaccessible because of an obstacle
                                case TARGET_INACC:
-                                       if (++target_inacc_cnt < 5) {
+                                       if (++target_inacc_cnt < 3) {
                                                FSM_TRANSITION(wait_and_try_again);
                                                ul_logerr("Inaccessible\n");
                                                FSM_TIMER(1000);
                                                /* FIXME (Filip): this could happen forever */
-                                       } else { /* go away if the point is not accessible, try max. 5x */
+                                       } else { /* go away if the point is not accessible, try max. 3x */
                                                target_inacc_cnt = 0;
                                                FSM_TRANSITION(wait_for_command);
                                                FSM_SIGNAL(MAIN, EV_MOTION_ERROR, NULL);