From a4f6eb79e9e5d0fda05083b4d21923d4c30eb431 Mon Sep 17 00:00:00 2001
From: Michal Vokac <vokac.m@gmail.com>
Date: Thu, 15 Dec 2011 01:50:52 +0100
Subject: [PATCH] robofsm: Start using camera recognition in demo FSM.

Add new FSM event on camera recognition done.
---
 src/robofsm/roboevent.py  |  1 +
 src/robofsm/robot_orte.c  | 12 ++++++++----
 src/robofsm/sub-states.cc | 25 ++++++++++++++++++-------
 3 files changed, 27 insertions(+), 11 deletions(-)

diff --git a/src/robofsm/roboevent.py b/src/robofsm/roboevent.py
index 9b77a23d..8845c74d 100644
--- a/src/robofsm/roboevent.py
+++ b/src/robofsm/roboevent.py
@@ -3,6 +3,7 @@ events = {
         "EV_START" : "Changed state of start connector.",
 
 	"EV_CRANE_DONE"	: "",
+	"EV_CAMERA_DONE" : "",
 	"EV_SWITCH_STRATEGY"	: "",
 
 	"EV_MOTION_DONE" 	: "Previously submitted motion is finished",
diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 4521cec9..28bd4322 100644
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -325,13 +325,17 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 			void *recvCallBackParam)
 {
 	struct camera_result_type *instance = (struct camera_result_type *)vinstance;
+        static bool last_response = false;
 
 	switch (info->status) {
 		case NEW_DATA: {
-			ROBOT_LOCK(camera_result);
-			// get result from camera
-			ROBOT_UNLOCK(camera_result);
-			robot.status[COMPONENT_CAMERA] = STATUS_OK;
+                        if (instance->data_valid && instance->data_valid != last_response) {
+                                ROBOT_LOCK(camera_result);
+                                robot.target_valid = instance->target_valid;
+                                ROBOT_UNLOCK(camera_result);
+                                FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
+                        }
+                        last_response = instance->data_valid;
 			break;
 		}
 		case DEADLINE:
diff --git a/src/robofsm/sub-states.cc b/src/robofsm/sub-states.cc
index a4878eec..7a17d971 100644
--- a/src/robofsm/sub-states.cc
+++ b/src/robofsm/sub-states.cc
@@ -37,16 +37,27 @@ FSM_STATE(recognize)
 {
         switch(FSM_EVENT) {
                 case EV_ENTRY:
-                        /*TODO
-                        - start camera recognition
-                        - if valid, touch target, else return target_valid = false
-                        - load target
-                        - return target_loaded = true
-                        */
-                        robot.target_valid = false;
+                        act_camera_on();
                         FSM_TIMER(1000);
                         break;
                 case EV_TIMER:
+                        FSM_TIMER(1000);
+                        break;
+                case EV_CAMERA_DONE:
+                        act_camera_off();
+                        if (robot.target_valid) {
+                                DBG_PRINT_EVENT("camera: Target valid!");
+                                FSM_TRANSITION(get_target_turn);
+                        } else {
+                                robot.target_loaded = false;
+                                SUBFSM_RET(NULL);
+                        }
+                        break;
+                case EV_EXIT:
+                        break;
+        }
+}
+
                         SUBFSM_RET(NULL);
                         break;
                 case EV_START:
-- 
2.39.2