From 3898447f950a15ba66aae5a30d97d6a21686c2f6 Mon Sep 17 00:00:00 2001
From: NunoDuarte <nunoduarte.cc.fduarte@gmail.com>
Date: Wed, 23 May 2018 20:13:29 +0100
Subject: [PATCH] improve the leader action and giving

---
 include/init.h           |  1 +
 src/extras/configure.cpp |  7 +++++--
 src/icub_leader.cpp      | 22 ++++++++++++----------
 3 files changed, 18 insertions(+), 12 deletions(-)

diff --git a/include/init.h b/include/init.h
index 2657f9f..e43dae7 100644
--- a/include/init.h
+++ b/include/init.h
@@ -80,6 +80,7 @@ class ControlThread: public RateThread
 
             // grasp once
             bool grasp;
+            bool released;
 
     public:
 
diff --git a/src/extras/configure.cpp b/src/extras/configure.cpp
index 8ba3dba..ed44422 100644
--- a/src/extras/configure.cpp
+++ b/src/extras/configure.cpp
@@ -208,12 +208,15 @@ using namespace yarp::math;
         // first, look down  
         look_down();
 
-        // initialize grasping counter
+        // initialize grasping and releasing counter
         grasp = false;
+        released = false;
 
         getchar();
         // Select Action
-        int num = 2; 
+        // giving  - 0
+        // placing - 1
+        int num = 1; 
 
         string Result; 
         std::ostringstream Result_string;
diff --git a/src/icub_leader.cpp b/src/icub_leader.cpp
index a09ed4c..7351916 100644
--- a/src/icub_leader.cpp
+++ b/src/icub_leader.cpp
@@ -142,16 +142,16 @@ using namespace std;
         }*/
 
         // begin
-        if (count < 1){
-            look = Eyes[0][1];
+        if (count < 40){
+            look = Eyes[0][0];
 
             fixate(look);
             yInfo()<<"fixating at ("<< look <<")";
 
         // duration of action
-        }else if (count > 1 and count < 200){
+        }else if (count > 40 and count < 1000){
 
-            look = Eyes[count-1][1];
+            look = Eyes[count][0];
 
             fixate(look);
             yInfo()<<"fixating at ("<< look <<")";
@@ -165,15 +165,16 @@ using namespace std;
             e[1] = xf[1] - p[1];
             e[2] = xf[2] - p[2];        
 
-            if (magnitude(e) < 0.1){
+            if (magnitude(e) < 0.1 and (not released)){
                 release("left");
+                released = true;
             }
 
 
         // finish
-        }else if (count > 2000){
+        }else if (count > 1000){
 
-            look = Eyes[999][1];
+            look = Eyes[999][0];
 
             fixate(look);
             yInfo()<<"fixating at ("<< look <<")";
@@ -186,8 +187,9 @@ using namespace std;
             e[1] = xf[1] - p[1];
             e[2] = xf[2] - p[2];        
 
-            if (magnitude(e) < 0.1){
-                release("left");
+            if (magnitude(e) < 0.1 and (not released)){
+                release("left");    
+                released = true;
             }
         }
 
@@ -241,7 +243,7 @@ int main(int argc, char *argv[])
     double startTime=Time::now();
     while(!done)
     {
-        if ((Time::now()-startTime)>50)
+        if ((Time::now()-startTime)>100)
             done=true;
     }