// TODO: Add all the State-Mehtods you need here
private int PerformAction(int currentState, object o)
{
//"grasp the cereal from the kitchen table and detect a person"
//go to kitchen
brain.SayAsync("I am going to the kitchen table.");
if (!cmdMan.HEAD_lookat(0, -1, 10000))
if (!cmdMan.HEAD_lookat(0, -1, 10000))
cmdMan.HEAD_lookat(0, -1, 10000);
if (!cmdMan.ARMS_goto("standby", 10000))
if (!cmdMan.ARMS_goto("standby", 10000))
cmdMan.ARMS_goto("standby", 10000);
if (!cmdMan.MVN_PLN_getclose("kitchentable", 10000))
if (!cmdMan.MVN_PLN_getclose("kitchentable", 10000))
cmdMan.MVN_PLN_getclose("kitchentable", 10000);
//find and take the cereal (try 3 times)
if (!cmdMan.ARMS_goto("home", 10000))
if (!cmdMan.ARMS_goto("home", 10000))
cmdMan.ARMS_goto("home", 10000);
//align to table
//find the cereal
int attemps = 0;
SM_SearchAndTakeObject.FinalStates state;
state = SM_SearchAndTakeObject.FinalStates.StillRunning;
while (attemps < 5 && state != SM_SearchAndTakeObject.FinalStates.OK)
{
SM_SearchAndTakeObject sm = new SM_SearchAndTakeObject(this.brain, this.cmdMan, false, new string[] { "cereal" }, 2);
state = sm.Execute();
if (state == SM_SearchAndTakeObject.FinalStates.OK)
this.armsOrder = sm.ArmsOrder;
attemps++;
}
//go to the bedroom
if (!cmdMan.ARMS_goto("standby", 10000))
if (!cmdMan.ARMS_goto("standby", 10000))
cmdMan.ARMS_goto("standby", 10000);
if (!cmdMan.HEAD_lookat(0, -1, 10000))
if (!cmdMan.HEAD_lookat(0, -1, 10000))
cmdMan.HEAD_lookat(0, -1, 10000);
if (!cmdMan.MVN_PLN_getclose("kitchen", 10000))
if (!cmdMan.MVN_PLN_getclose("kitchen", 10000))
cmdMan.MVN_PLN_getclose("kitchen", 10000);
if (!cmdMan.HEAD_lookat(0, 0, 10000))
if (!cmdMan.HEAD_lookat(0, 0, 10000))
cmdMan.HEAD_lookat(0, 0, 10000);
if (!cmdMan.ARMS_goto("home", 10000))
if (!cmdMan.ARMS_goto("home", 10000))
cmdMan.ARMS_goto("home", 10000);
return (int)States.FindPerson;
}
// TODO: Add all the State-Mehtods you need here
private int PerformAction(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Initializing test.");
//"take the jam from the side table and deliver it to the shelf"
//go to side table
if (!cmdMan.HEAD_lookat(0, -1, 10000))
if (!cmdMan.HEAD_lookat(0, -1, 10000))
cmdMan.HEAD_lookat(0, -1, 10000);
if (!cmdMan.ARMS_goto("standby", 10000))
if (!cmdMan.ARMS_goto("standby", 10000))
cmdMan.ARMS_goto("standby", 10000);
if (!cmdMan.MVN_PLN_getclose("sidetable", 10000))
if (!cmdMan.MVN_PLN_getclose("sidetable", 10000))
cmdMan.MVN_PLN_getclose("sidetable", 10000);
//find and take the jam (try 3 times)
if (!cmdMan.ARMS_goto("home", 10000))
if (!cmdMan.ARMS_goto("home", 10000))
cmdMan.ARMS_goto("home", 10000);
//////////////////////////align to table
//find the cereal
int attemps = 0;
SM_SearchAndTakeObject.FinalStates state;
state = SM_SearchAndTakeObject.FinalStates.StillRunning;
while (attemps < 5 || state != SM_SearchAndTakeObject.FinalStates.OK)
{
SM_SearchAndTakeObject sm = new SM_SearchAndTakeObject(this.brain, this.cmdMan, false, new string[] { "jam" }, 2);
state = sm.Execute();
if (state == SM_SearchAndTakeObject.FinalStates.OK)
this.armsOrder = sm.ArmsOrder;
attemps++;
}
//go to the shelf
if (!cmdMan.ARMS_goto("standby", 10000))
if (!cmdMan.ARMS_goto("standby", 10000))
cmdMan.ARMS_goto("standby", 10000);
if (!cmdMan.HEAD_lookat(0, -1, 10000))
if (!cmdMan.HEAD_lookat(0, -1, 10000))
cmdMan.HEAD_lookat(0, -1, 10000);
if (!cmdMan.MVN_PLN_getclose("shelf", 10000))
if (!cmdMan.MVN_PLN_getclose("shelf", 10000))
cmdMan.MVN_PLN_getclose("shelf", 10000);
//////////////////////////align to shelf
brain.SayAsync("i am going to drop the jam");
Thread.Sleep(1000);
//bring the cereal
cmdMan.ST_PLN_drop(armsOrder[0], 30000);
if (!cmdMan.ARMS_goto("standby", 10000))
if (!cmdMan.ARMS_goto("standby", 10000))
cmdMan.ARMS_goto("standby", 10000);
return (int)States.FinalState;
}
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;
}
private int SearchAndTakeObject(int currentState, object o)
{
//currentOrder = orders.Dequeue();
SM_SearchAndTakeObject searchAndTakeObjectSM = new SM_SearchAndTakeObject(brain, cmdMan, false, currentOrder.objectName);
if (searchAndTakeObjectSM.Execute() != SM_SearchAndTakeObject.FinalStates.OK)
{
TextBoxStreamWriter.DefaultLog.WriteLine("Could not fetch order: " + currentOrder.objectName + " for " + currentOrder.personName);
brain.SayAsync("I could not get the order for " + currentOrder.personName);
cmdMan.MVN_PLN_move(-0.4, 250000);
if (ordersCount < 3)
{
attemptCounter = 0;
ordersCount++;
return (int)States.GoToPartyRoom;
}
return (int)States.LeaveArena;
}
cmdMan.MVN_PLN_move(-0.4, 250000);
return (int)States.GoToHumanLocation;
}
