robofsm: Catch the cans, if obstacle is detected

[eurobot/public.git] / src / robofsm / common-states.cc

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index c36f04f2882517d8ebe62e58d4490875d9f0ed7d..2559f8bfcccf11d7e4b67d0c0b9e7a2a17c58139 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -100,6 +100,10 @@ FSM_STATE(go_to_D5)
                         break;
                 case EV_RETURN:
                         break;
+                case EV_MOTION_OBSTACLE:
+                        DBG_PRINT_EVENT("catch the cans");
+                        act_jaws(CATCH);
+                        break;
                 case EV_MOTION_ERROR:
                         DBG_PRINT_EVENT("can not go to D5");
                         DBG_PRINT_EVENT("go to D1");
@@ -151,6 +155,10 @@ FSM_STATE(go_to_D1)
                         break;
                 case EV_RETURN:
                         break;
+                case EV_MOTION_OBSTACLE:
+                        DBG_PRINT_EVENT("catch the cans");
+                        act_jaws(CATCH);
+                        break;
                 case EV_MOTION_ERROR:
                         DBG_PRINT_EVENT("can not go to D1");
                         DBG_PRINT_EVENT("go to house");
@@ -204,6 +212,10 @@ FSM_STATE(go_to_house_1)
                         break;
                 case EV_RETURN:
                         break;
+                case EV_MOTION_OBSTACLE:
+                        DBG_PRINT_EVENT("catch the cans");
+                        act_jaws(CATCH);
+                        break;
                 case EV_MOTION_ERROR:
                         DBG_PRINT_EVENT("can not go to house 1");
                         DBG_PRINT_EVENT("repeat go to house");