Merge pull request #90 from BCLab-UNM/bug-pickup_efficiency

Fixes a bug twitching of gripper as well as improving pickup performance.

Author: gmfricke

src/behaviours/src/PickUpController.cpp

@@ -2,7 +2,8 @@
 #include <limits> // For numeric limits
 #include <cmath> // For hypot
 
-PickUpController::PickUpController() {
+PickUpController::PickUpController()
+{
   lockTarget = false;
   timeOut = false;
   nTargetsSeen = 0;
@@ -19,42 +20,54 @@ PickUpController::PickUpController() {
   result.PIDMode = SLOW_PID;
 }
 
-PickUpController::~PickUpController() {
-}
+PickUpController::~PickUpController() { /*Destructor*/  }
 
 void PickUpController::SetTagData(vector<Tag> tags)
 {
 
-  if (tags.size() > 0) {
+  if (tags.size() > 0)
+  {
 
     nTargetsSeen = tags.size();
 
+    //we saw a target, set target_timer
+    target_timer = current_time;
+
     double closest = std::numeric_limits<double>::max();
     int target  = 0;
-    for (int i = 0; i < tags.size(); i++) { //this loop selects the closest visible block to makes goals for it
 
-      if (tags[i].getID() == 0) {
+    //this loop selects the closest visible block to makes goals for it
+    for (int i = 0; i < tags.size(); i++)
+    {
+
+      if (tags[i].getID() == 0)
+      {
 
         targetFound = true;
 
-        double test = hypot(hypot(tags[i].getPositionX(), tags[i].getPositionY()), tags[i].getPositionZ()); //absolute distance to block from camera lens
+        //absolute distance to block from camera lens
+        double test = hypot(hypot(tags[i].getPositionX(), tags[i].getPositionY()), tags[i].getPositionZ());
+
         if (closest > test)
         {
           target = i;
           closest = test;
         }
       }
-      else {
-        nTargetsSeen--;
+      else
+      {
 
         if(tags[i].getID() == 256)
         {
+
           Reset();
+
           if (has_control)
           {
             cout << "pickup reset return interupt free" << endl;
             release_control = true;
           }
+
           return;
         }
       }
@@ -65,9 +78,6 @@ void PickUpController::SetTagData(vector<Tag> tags)
     ///TODO: Explain the trig going on here- blockDistance is c, 0.195 is b; find a
     blockDistanceFromCamera = hypot(hypot(tags[target].getPositionX(), tags[target].getPositionY()), tags[target].getPositionZ());
 
-    //WRONG WRONG WRONG WRONG
-    //blockDistance = hypot(tags[target].getPositionZ(), tags[target].getPositionY()); //distance from bottom center of chassis ignoring height.
-
     if ( (blockDistanceFromCamera*blockDistanceFromCamera - 0.195*0.195) > 0 )
     {
       blockDistance = sqrt(blockDistanceFromCamera*blockDistanceFromCamera - 0.195*0.195);
@@ -92,7 +102,8 @@ void PickUpController::SetTagData(vector<Tag> tags)
 bool PickUpController::SetSonarData(float rangeCenter)
 {
 
-  if (rangeCenter < 0.12 && targetFound) {
+  if (rangeCenter < 0.12 && targetFound)
+  {
     result.type = behavior;
     result.b = nextProcess;
     result.reset = true;
@@ -106,17 +117,21 @@ bool PickUpController::SetSonarData(float rangeCenter)
 
 void PickUpController::ProcessData()
 {
+
   if(!targetFound)
   {
+    //cout << "PICKUP No Target Seen!" << endl;
+
     // Do nothing
     return;
   }
 
-  //if target is close enough
   //diffrence between current time and millisecond time
   long int Tdiff = current_time - millTimer;
   float Td = Tdiff/1e3;
 
+  //cout << "PICKUP Target Seen!" << endl;
+
   //cout << "distance : " << blockDistanceFromCamera << " time is : " << Td << endl;
 
   if (blockDistanceFromCamera < 0.14 && Td < 3.9)
@@ -147,30 +162,34 @@ bool PickUpController::ShouldInterrupt(){
     return true;
   }
 
-  if ((targetFound && !interupted) || targetHeld) {
+  if ((targetFound && !interupted) || targetHeld)
+  {
     interupted = true;
     has_control = false;
     return true;
   }
-  else if (!targetFound && interupted) {
+  else if (!targetFound && interupted)
+  {
     interupted = false;
     has_control = false;
     return true;
   }
-  else {
+  else
+  {
     return false;
   }
 }
 
-Result PickUpController::DoWork() {
+Result PickUpController::DoWork()
+{
 
   has_control = true;
 
-  if (!targetHeld) {
+  if (!targetHeld)
+  {
     //threshold distance to be from the target block before attempting pickup
     float targetDistance = 0.15; //meters
 
-
     // -----------------------------------------------------------
     // millisecond time = current time if not in a counting state
     //     when timeOut is true, we are counting towards a time out
@@ -208,13 +227,27 @@ Result PickUpController::DoWork() {
     // If we don't see any blocks or cubes turn towards the location of the last cube we saw.
     // I.E., try to re-aquire the last cube we saw.
 
-    float grasp_time_begin = 1.7;
-    float raise_time_begin = 2.5;
+    float grasp_time_begin = 1.1;
+    float raise_time_begin = 2.0;
     float lower_gripper_time_begin = 4.0;
     float target_reaquire_begin= 4.2;
     float target_pickup_task_time_limit = 4.8;
 
     //cout << "blockDistance DOWORK:  " << blockDistance << endl;
+
+    //Calculate time difference between last seen tag
+    float target_timeout = (current_time - target_timer)/1e3;
+
+    //delay between the camera refresh and rover runtime is 6/10's of a second
+    float target_timeout_limit = 0.61;
+
+    //Timer to deal with delay in refresh from camera and the runtime of rover code
+    if( target_timeout >= target_timeout_limit )
+    {
+        //Has to be set back to 0
+        nTargetsSeen = 0;
+    }
+
 
     if (nTargetsSeen == 0 && !lockTarget)
     {
@@ -223,7 +256,7 @@ Result PickUpController::DoWork() {
       {
         result.pd.cmdVel = 0.0;
         result.pd.cmdAngularError= 0.0;
-        result.wristAngle = 0.8;
+        result.wristAngle = 1.25;
         // result.fingerAngle does not need to be set here
 
         // We are getting ready to start the pre-programmed pickup routine now! Maybe? <(^_^)/"
@@ -252,7 +285,7 @@ Result PickUpController::DoWork() {
       result.pd.cmdVel = vel;
       result.pd.cmdAngularError = -blockYawError;
       timeOut = false;
-      nTargetsSeen = 0;
+
       return result;
     }
     else if (!lockTarget) //if a target hasn't been locked lock it and enter a counting state while slowly driving forward.
@@ -279,11 +312,14 @@ Result PickUpController::DoWork() {
 
 
     // the magic numbers compared to Td must be in order from greater(top) to smaller(bottom) numbers
-    if (Td > target_reaquire_begin && timeOut) {
+    if (Td > target_reaquire_begin && timeOut)
+    {
       lockTarget = false;
       ignoreCenterSonar = true;
     }
-    else if (Td > lower_gripper_time_begin && timeOut) //if enough time has passed enter a recovery state to re-attempt a pickup
+
+    //if enough time has passed enter a recovery state to re-attempt a pickup
+    else if (Td > lower_gripper_time_begin && timeOut)
     {
       result.pd.cmdVel = -0.15;
       result.pd.cmdAngularError= 0.0;
@@ -292,8 +328,8 @@ Result PickUpController::DoWork() {
       result.wristAngle = 0;
     }
 
-
-    if (Td > target_pickup_task_time_limit && timeOut) //if no targets are found after too long a period go back to search pattern
+    //if no targets are found after too long a period go back to search pattern
+    if (Td > target_pickup_task_time_limit && timeOut)
     {
       Reset();
       interupted = true;
@@ -306,7 +342,8 @@ Result PickUpController::DoWork() {
   return result;
 }
 
-bool PickUpController::HasWork() {
+bool PickUpController::HasWork()
+{
   return targetFound;
 }
 

src/behaviours/src/PickUpController.h

@@ -44,6 +44,7 @@ class PickUpController : virtual Controller
   bool timeOut;
   int nTargetsSeen;
   long int millTimer;
+  long int target_timer;
 
   //yaw error to target block
   double blockYawError;