int TakeObject(int currentState, object o)
{
if (ObjectToFind.Substring(0, 7).ToLower().Equals("unknown"))
{
SayObjectName = "unknown object";
}
else
{
SayObjectName = ObjectToFind;
}
if (this.succesGetClose)
{
brain.SayAsync("I will take the " + SayObjectName);
Thread.Sleep(5000);
if (cmdMan.ST_PLN_takeobject(ObjectToFind, 105000))
{
TextBoxStreamWriter.DefaultLog.WriteLine("object taken");
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("cant take object, using takehandover");
brain.SayAsync(" I cant take the " + SayObjectName);
cmdMan.ST_PLN_takehandover(ObjectToFind, 40000);
}
}
else
{
Thread.Sleep(5000);
TextBoxStreamWriter.DefaultLog.WriteLine("Getclose not succesfull, using takehandover");
brain.SayAsync("I can't take the " + SayObjectName);
cmdMan.ST_PLN_takehandover(ObjectToFind, 40000);
}
this.finalState = FinalStates.OK;
return((int)States.FinalState);
}
int TakeObject(int currentState, object o)
{
if (ObjectToFind.Length >= 7 && ObjectToFind.Substring(0, 7).ToLower().Equals("unknown"))
{
SayObjectName = "unknown object";
}
else
{
SayObjectName = ObjectToFind;
}
brain.SayAsync("I will take the " + SayObjectName);
Thread.Sleep(5000);
if (cmdMan.ST_PLN_takeobject(ObjectToFind, 180000))
{
TextBoxStreamWriter.DefaultLog.WriteLine("object taken");
}
else
{
if (useTakeHandOver)
{
TextBoxStreamWriter.DefaultLog.WriteLine("cant take object, using takehandover");
brain.SayAsync(" I cant reach the " + SayObjectName);
cmdMan.ST_PLN_takehandover(ObjectToFind, 40000);
}
else
{
brain.SayAsync("I cant reach the " + SayObjectName + ", I will look for another object.");
objectsFound.RemoveAt(objectFoundIndex);
TextBoxStreamWriter.DefaultLog.WriteLine("Cant take the object, trying to take another one.");
return((int)States.SetObjectToTake);
}
}
this.finalState = FinalStates.OK;
return((int)States.FinalState);
}
int TakeObject(int currentState, object o)
{
string leftArm = "", rightArm = "";
if (firstObjectTaken)
{
armToUse = (laTaken) ? "right" : "left";
}
else
{
armToUse = "";
}
armToUse = "left";
if (objectsToFind[takeCounter].Length >= 7 && objectsToFind[takeCounter].Substring(0, 7).ToLower().Equals("unknown"))
{
SayObjectName = "unknown object";
}
else
{
SayObjectName = objectsToFind[takeCounter];
}
//TEMP
//brain.SayAsync("I will take the object");
//Thread.Sleep(10000);
//reemplazar por bnuevo comando para forzar a que tome un objeto con un brazo dado
//if (cmdMan.ST_PLN_takeobject(objectsToFind[takeCounter], armToUse, out string, 180000))
//if (cmdMan.ST_PLN_takeobject(objectsToFind[takeCounter], 180000))
cmdMan.ST_PLN_takeobject(objectsToFind[takeCounter], armToUse, out leftArm, out rightArm, 180000);
if (!(leftArm == "empty" && rightArm == "empty"))
{
brain.SayAsync("I got the " + SayObjectName);
TextBoxStreamWriter.DefaultLog.WriteLine("object taken: " + objectsToFind[takeCounter]);
foundObjects[takeCounter] = objectsToFind[takeCounter];
//arctualizar la bandera del brazo cupado laTaken}
laTaken = (leftArm == "empty") ? false : true;
if (!firstObjectTaken)
{
armsOrder[0] = (laTaken) ? "left" : "right";
}
else
{
armsOrder[1] = (armsOrder[0] == "right") ? "left" : "right";
}
TextBoxStreamWriter.DefaultLog.WriteLine(objectsToFind[takeCounter] + " using the " + ((!firstObjectTaken) ? armsOrder[0] : armsOrder[1]) + " arm");
}
else
{
if (useTakeHandOver)
{
TextBoxStreamWriter.DefaultLog.WriteLine("cant take object, using takehandover");
brain.SayAsync(" I cant reach the " + SayObjectName);
//brain.SayAsync("I will point at the " + objectsToFind[takeCounter] + ".");
//TODO: point at object
cmdMan.ST_PLN_takehandover(objectsToFind[takeCounter], 40000);
foundObjects[takeCounter] = objectsToFind[takeCounter];
//arctualizar la bandera del brazo cupado laTaken
laTaken = (leftArm == "empty") ? false : true;
if (!firstObjectTaken)
{
armsOrder[0] = (laTaken) ? "left" : "right";
}
else
{
armsOrder[1] = (armsOrder[0] == "right") ? "left" : "right";
}
}
else
{
finalState = FinalStates.FoundButNotTaken;
foundObjects[takeCounter] = "";
//TEMP
//brain.SayAsync("I cant reach the " + SayObjectName + ", I will look for another object.");
//brain.SayAsync("I cant reach the object, I will look for another object.");
TextBoxStreamWriter.DefaultLog.WriteLine("Cant take the object, trying to take another one.");
objectsFound.RemoveAt(0);
return((int)States.SetObjectToTake);
}
}
if (!firstObjectTaken && twoArms)
{
TextBoxStreamWriter.DefaultLog.WriteLine("First object Taken.");
/*despues de la primer ejecucion checar con que brazo tomó el objeto y actualizar la bandera laTaken(true, toma con el brazo izq, else si no )*/
//laTaken=false
takeCounter++;
firstObjectTaken = true;
objectsFound.RemoveAt(0);
return((int)States.SetObjectToTake);
}
this.finalState = FinalStates.OK;
return((int)States.FinalState);
}