robofsm: Back switches handling

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index 936d175c55630012c2e82feba6ae7936fe40815a..74b3e48c5340cd47cf55ff8e745e293b1f7a909b 100644 (file)
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -262,6 +262,9 @@ FSM_STATE(movement)
                                case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
                        }
                        break;
+               case EV_OBSTACLE_BEHIND:
+                       FSM_TRANSITION(wait_and_try_again);
+                       break;
                case EV_TRAJECTORY_LOST:
                        FSM_TRANSITION(lost);
                        break;
@@ -289,6 +292,9 @@ FSM_STATE(close_to_target)
                        FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
                        FSM_TRANSITION(wait_for_command);
                        break;
+               case EV_OBSTACLE_BEHIND:
+                       FSM_TRANSITION(wait_and_try_again);
+                       break;
                case EV_TRAJECTORY_LOST:
                case EV_TIMER:
                        FSM_TRANSITION(lost);
@@ -327,6 +333,7 @@ FSM_STATE(lost)
                        break;
                case EV_EXIT:
                        break;
+               case EV_OBSTACLE_BEHIND:
                case EV_TRAJECTORY_DONE:
                case EV_NEW_TARGET:
                case EV_TRAJECTORY_DONE_AND_CLOSE:
@@ -359,6 +366,8 @@ FSM_STATE(wait_and_try_again)
                case EV_MOVE_STOP:
                        FSM_TRANSITION(wait_for_command);
                        break;
+               case EV_OBSTACLE_BEHIND:
+                       break;
                case EV_NEW_TARGET:
                case EV_TRAJECTORY_LOST:
                case EV_RETURN:

diff --git a/src/robofsm/roboevent.py b/src/robofsm/roboevent.py
index 8771645d27a7dbb8bc5ed563e79fd822f92b2a6c..20ba59afdef32581d4d8d7a53f23ca31a15d78c4 100644 (file)
--- a/src/robofsm/roboevent.py
+++ b/src/robofsm/roboevent.py
@@ -28,6 +28,7 @@ events = {
         "EV_TRAJECTORY_DONE" : "Reference position is at the end of the previously submitted trajectory",
        "EV_TRAJECTORY_DONE_AND_CLOSE" : "::EV_TRAJECTORY_DONE + distance of actual position to the target is less than XXX",
        "EV_OBSTACLE" : "An obstacle is detected on actual trajectory - we must avoid it.",
+       "EV_OBSTACLE_BEHIND" : "",
        #"EV_FOUND_AFTER_RESET" : "MCL estimated position after reset",
        #"EV_RECALCULATION_FAILED" : "Trajectory recalculation caused by a detected obstcale has failed (error or target not reachable)",
     },

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 50248dd454e85c765191f6ada835ae899ecb147d..cc98ee4140da17b92d1e5950e2ea17a05b6ec73e 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -320,8 +320,10 @@ void rcv_puck_inside_cb(const ORTERecvInfo *info, void *vinstance,
        static bool previous_switch_status;
        switch (info->status) {
        case NEW_DATA: {
-                       if (robot.orte.puck_inside.status == 1 && previous_switch_status != 1)
-                               FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
+               if (robot.orte.puck_inside.status == 1 && previous_switch_status != 1) {
+                       //FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
+                       FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
+               }
                        previous_switch_status = robot.orte.puck_inside.status;
                        robot.hw_status[STATUS_CHE]=HW_STATUS_OK;
                }