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); }