private int SearchAndTakeObject(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> SearchAndTakeObject state reached.");
SM_SearchAndTakeObject sm = new SM_SearchAndTakeObject(this.brain, this.cmdMan, false, new string[] { this.objectToTake, "" }, 2, false);
SM_SearchAndTakeObject.FinalStates SATO_FinalState = sm.Execute();
if (SATO_FinalState == SM_SearchAndTakeObject.FinalStates.OK)
{
SMConfiguration.ARMS_usedArm = sm.ArmsOrder[0];
return((int)States.SATO_Succeeded);
}
else
{
return((int)States.SATO_Failed);
}
}
/// <summary>
/// The robot tries to find an object and take it
/// </summary>
private int SearchAndTakeObject(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> State reached SearchAndTakeObject");
SM_SearchAndTakeObject smSearchAndTakeObject = new SM_SearchAndTakeObject(brain, cmdMan, true, new string[] { "objects", "objects" }, false, 1, false);
SM_SearchAndTakeObject.FinalStates searchAndTake_finalState = smSearchAndTakeObject.Execute();
armsOrder = smSearchAndTakeObject.ArmsOrder;
if (searchAndTake_finalState == SM_SearchAndTakeObject.FinalStates.OK)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Object succesfully taken.");
attemptCounter = 0;
objectTaken = true;
cmdMan.MVN_PLN_move(-0.35, 60000);
if (!this.cmdMan.ARMS_goto(SMConfiguration.ArmsObjectTakenPosition, 10000))
{
if (!this.cmdMan.ARMS_goto(SMConfiguration.ArmsObjectTakenPosition, 10000))
{
this.cmdMan.ARMS_goto(SMConfiguration.ArmsObjectTakenPosition, 10000);
}
}
return((int)States.GetCloseToTable);
}
if (attemptCounter < 3)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Can't find and take the object. Trying again.");
attemptCounter++;
return((int)States.SearchAndTakeObject);
}
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Can't find and take the object. Will try to continue with the test.");
attemptCounter = 0;
return((int)States.GetCloseToTable);
}