From 72a6450ac6531b4fa23fda42a74587cb310f8788 Mon Sep 17 00:00:00 2001
From: Michal Vokac <vokac.m@gmail.com>
Date: Sat, 6 Oct 2012 01:03:43 +0200
Subject: [PATCH] robofsm: Strategy automaton tuning.

---
 src/robofsm/common-states.cc |  2 +-
 src/robofsm/roboevent.py     |  3 +-
 src/robofsm/robot.h          |  1 +
 src/robofsm/sub-states.cc    | 56 ++++++++++++++++++++++++++++++++++--
 src/robofsm/sub-states.h     |  1 +
 5 files changed, 59 insertions(+), 4 deletions(-)

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 1a852a09..50e44f63 100644
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -296,7 +296,7 @@ FSM_STATE(move_around)
                 case EV_ENTRY:
                         do {
                                 goalx = ((rand()%PLAYGROUND_HEIGHT_MM)/1000.0);
-                                goaly += ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
+                                goaly = ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
                         } while (!ShmapIsFreePoint(goalx, goaly) && close_goal(goalx, goaly));
 
                         robot_goto_notrans(goalx, goaly, NO_TURN(), &tcSlow);
diff --git a/src/robofsm/roboevent.py b/src/robofsm/roboevent.py
index 3a10f03d..e723d5c9 100644
--- a/src/robofsm/roboevent.py
+++ b/src/robofsm/roboevent.py
@@ -4,7 +4,8 @@ events = {
 
 	"EV_JAWS_DONE"  	: "",
 	"EV_LIFT_DONE"          : "",
-	"EV_CAMERA_DONE" : "",
+	"EV_CAMERA_DONE"        : "",
+        "EV_OMRON_DONE"         : "",
 	"EV_SWITCH_STRATEGY"	: "",
 
 	"EV_MOTION_DONE" 	: "Previously submitted motion is finished",
diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 31be9afd..f7f69755 100644
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -210,6 +210,7 @@ struct robot {
         /* variables for target detection */
         bool target_loaded;
         bool target_valid;
+        int target_ang;
         char target_color[3];
 
 	struct map *map;	/* Map for pathplanning (no locking) */
diff --git a/src/robofsm/sub-states.cc b/src/robofsm/sub-states.cc
index ce5aac10..b8540666 100644
--- a/src/robofsm/sub-states.cc
+++ b/src/robofsm/sub-states.cc
@@ -74,6 +74,11 @@ void get_hokuyo_min(double *alpha, double *minimum)
         printf("alpha: %f, beta: %f, minimum: %f\n", *alpha, beta, *minimum);
 }
 
+void get_camera_ang(double *alpha)
+{
+        *alpha = DEG2RAD(robot.target_ang);
+}
+
 void enable_obstacle_avoidance(bool enable)
 {
         robot.obstacle_avoidance_enabled = enable;
@@ -106,6 +111,7 @@ FSM_STATE(recognize)
                 case EV_CAMERA_DONE:
                         act_camera_off();
                         if (robot.target_valid) {
+                                robot.target_valid = false;
                                 DBG_PRINT_EVENT("camera: Target valid!");
                                 FSM_TRANSITION(get_target_turn);
                         } else {
@@ -125,12 +131,23 @@ FSM_STATE(get_target_turn)
         const double close = 0.05;
         double alpha, minimum;
         double x, y, phi;
+        static char first = 1;
 
         switch(FSM_EVENT) {
                 case EV_ENTRY:
                         DBG_PRINT_EVENT("turn");
-                        enable_obstacle_avoidance(false);
-                        get_hokuyo_min(&alpha, &minimum);
+                        //enable_obstacle_avoidance(false);
+
+                        if (first) {
+                                // use angle from camera on first run
+                                get_camera_ang(&alpha);
+                                minimum = 1;
+                                first = 0;
+                        } else {
+                                // than navigate using hokuyo
+                                get_hokuyo_min(&alpha, &minimum);
+                        }
+
                         robot_get_est_pos(&x, &y, &phi);
                         robot_move_by(0, TURN(alpha+phi), &tcVerySlow);
                         break;
@@ -178,6 +195,41 @@ FSM_STATE(get_target_touch)
 }
 
 FSM_STATE(get_target_load)
+{
+        static char delay = 0;
+
+        switch(FSM_EVENT) {
+                case EV_ENTRY:
+                        act_jaws(CATCH);
+                        FSM_TIMER(1000);
+                        break;
+                case EV_TIMER:
+                        if (++delay > 3) {
+                                // target was not confirmed
+                                DBG_PRINT_EVENT("OMRON: Target NOT valid!");
+                                act_jaws(OPEN);
+                                robot.target_loaded = false;
+                                robot.target_valid  = false;
+                                FSM_TRANSITION(get_target_unload);
+                        } else {
+                                FSM_TIMER(1000);
+                        }
+                        break;
+                case EV_OMRON_DONE:
+                        // target confirmed as valid by OMRON sensor
+                        // return to main automaton and transport target to storage
+                        DBG_PRINT_EVENT("OMRON: Target valid!");
+                        robot.target_loaded = true;
+                        robot.target_valid  = true;
+                        SUBFSM_RET(NULL);
+                        break;
+                case EV_EXIT:
+                        break;
+        }
+}
+
+/* unload target if not valid = no color match using OMRON */
+FSM_STATE(get_target_unload)
 {
         static int direction = 0;
 
diff --git a/src/robofsm/sub-states.h b/src/robofsm/sub-states.h
index 664fc2eb..5362c2e6 100644
--- a/src/robofsm/sub-states.h
+++ b/src/robofsm/sub-states.h
@@ -14,6 +14,7 @@ FSM_STATE_DECL(recognize);
 FSM_STATE_DECL(get_target_turn);
 FSM_STATE_DECL(get_target_touch);
 FSM_STATE_DECL(get_target_load);
+FSM_STATE_DECL(get_target_unload);
 FSM_STATE_DECL(get_target_back);
 
 void get_hokuyo_min(double *alpha, double *minimum);
-- 
2.39.2