robofsm: Wait max 10 seconds for camera recognition.

diff --git a/src/robofsm/sub-states.cc b/src/robofsm/sub-states.cc
index 5f096c666a43994c378eb6110fa6d623d3f46e28..ce5aac10d9fe54139f887ac2b15023c7ac0001d1 100644 (file)
--- a/src/robofsm/sub-states.cc
+++ b/src/robofsm/sub-states.cc
@@ -83,6 +83,8 @@ void enable_obstacle_avoidance(bool enable)
 
 FSM_STATE(recognize)
 {
+        static int camera_delay = 0;
+
         switch(FSM_EVENT) {
                 case EV_ENTRY:
                         act_camera_recognize(robot.target_color);
@@ -90,7 +92,16 @@ FSM_STATE(recognize)
                         FSM_TIMER(1000);
                         break;
                 case EV_TIMER:
-                        FSM_TIMER(1000);
+                        if (++camera_delay > 10) {
+                                // waiting for 10 seconds now, return back
+                                DBG_PRINT_EVENT("camera: No response!");
+                                ROBOT_LOCK(camera_result);
+                                robot.target_valid = false;
+                                ROBOT_UNLOCK(camera_result);
+                                SUBFSM_RET(NULL);
+                        } else {
+                                FSM_TIMER(1000);
+                        }
                         break;
                 case EV_CAMERA_DONE:
                         act_camera_off();
@@ -104,6 +115,7 @@ FSM_STATE(recognize)
                         }
                         break;
                 case EV_EXIT:
+                        camera_delay = 0;
                         break;
         }
 }