robofsm: Start using camera recognition in FSM.

Add new FSM event on camera recognition done.

diff --git a/src/robofsm/roboevent.py b/src/robofsm/roboevent.py
index e70c1c7ea9d5218fd647dfd2d75ad3debab8ce17..3a10f03d742bccdeac5de11c72d8f87d86c931df 100644 (file)
--- a/src/robofsm/roboevent.py
+++ b/src/robofsm/roboevent.py
@@ -4,6 +4,7 @@ events = {
 
        "EV_JAWS_DONE"          : "",
        "EV_LIFT_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 dd257bf846893e026d260a0f242fdd3b50487061..7f92c21dd4b82890a12fd9091ebd23cc5331ae94 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -345,13 +345,13 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 
        switch (info->status) {
                case NEW_DATA: {
-                        if (instance->info && (instance->info != last_response)) {
+                        if (instance->data_valid && instance->data_valid != last_response) {
                                 ROBOT_LOCK(camera_result);
-                                robot.target_valid = instance->info;
+                                robot.target_valid = instance->target_valid;
                                 ROBOT_UNLOCK(camera_result);
                                 FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
                         }
-                        last_response = instance->info;
+                        last_response = instance->data_valid;
                        break;
                }
                case DEADLINE:

diff --git a/src/robofsm/sub-states.cc b/src/robofsm/sub-states.cc
index f970b50a3910d60d12f8fad0e36226194a420747..41f8d77e16a62b31f6201744903d8754aa120e94 100644 (file)
--- 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_recognize(robot.target_color);
                         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:

diff --git a/src/types/robottype.idl b/src/types/robottype.idl
index ba55abc8ce394dd089337742ac5560fdb69b3156..e3725408739a974170a8844337fba52764ff32f2 100644 (file)
--- a/src/types/robottype.idl
+++ b/src/types/robottype.idl
@@ -152,7 +152,8 @@ module camera {
                 octet x;
                 octet y;
 
-                octet info;
+                boolean data_valid;
+                boolean target_valid;
                 error error;
        };
 };