int GetCloseObjectLocation(int currentState, object o)
{
cmdMan.HEAD_lookat(0, 0, 5000);
TextBoxStreamWriter.DefaultLog.WriteLine("Getting close to: " + this.objectLocation);
if (cmdMan.MVN_PLN_getclose(this.objectLocation, 600000))
{
cmdMan.SPG_GEN_asay("i have arrived to the " + this.objectLocation, 10000);
TextBoxStreamWriter.DefaultLog.WriteLine("Location reached: " + this.objectLocation);
this.attemptCounter = 0;
return((int)States.SearchCloseObjects);
}
if (attemptCounter < 3)
{
this.attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("Cant get close to the specified location, trying again.");
/// para interface
cmdMan.SPG_GEN_asay("I can't get close to the" + this.objectLocation, 10000);
return(currentState);
}
//this.brain.SayAsync("I could not get close to the specified location");
this.attemptCounter = 0;
TextBoxStreamWriter.DefaultLog.WriteLine("Cant get close to the specified location, will try to continue with the test.");
return((int)States.SearchCloseObjects);
}
/// <summary>
/// The robot navigates to a given table to take an order, if there is no more tables to visit then
/// the robot goes to the kitchen to take the food/drinks/etc
/// </summary>
private int GoToOrderTable(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> GoToOrderTable state rached.");
//TODO:Launch a thread to detect request while navigating
if (tablesToVisit.Count > 0)
{
//obtain the current table MVN_PLN location to visit
MapLocation tableLocation = tablesToVisit.Dequeue();
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Going to: " + tableLocation.Name);
//navigate to the current table
brain.SayAsync(SMConfiguration.builTableVisitMessage(tableLocation.Alias));
if (!cmdMan.MVN_PLN_getclose(tableLocation.Name, 20000))
{
if (!cmdMan.MVN_PLN_getclose(tableLocation.Name, 20000))
{
cmdMan.MVN_PLN_getclose(tableLocation.Name, 20000);
}
}
//TODO: ask for an order
return((int)States.FinalState);
}
MapLocation kitchenLocation = SMConfiguration.MvnPlnKitchenLocation;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Going to: " + SMConfiguration.MvnPlnKitchenLocation.Name);
cmdMan.SPG_GEN_asay(SMConfiguration.GoingToKitchenMessage, 5000);
//TODO: go to the kitchen to grasp object
return((int)States.FinalState);
}
/// <summary>
/// The robot navigates to a predefined location in MVN_PPLN, then aligns with a table using vision
/// the MVN_PLN location must be in front of a table
/// </summary>
/// <returns>The next state: searchAndTakeObject if the robot doesn't have an object in his hand,
/// DropObject if the robot has an object in his hand</returns>
private int GetCloseToTable(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> State reached GetCloseToTable");
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Going to location: " + SMConfiguration.ObjectTableLocation);
if (objectTaken)
{
locationToReach = SMConfiguration.DropTableLocation;
}
else
{
locationToReach = SMConfiguration.ObjectTableLocation;
}
if (brain.GetCloseToTable(locationToReach, 15000))
{
cmdMan.SPG_GEN_asay(SMConfiguration.TableArrivedMessage, 2000);
attemptCounter = 0;
if (objectTaken)
{
return((int)States.DropObject);
}
else
{
return((int)States.SearchAndTakeObject);
}
}
if (attemptCounter < 3)
{
attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Can't reach location, trying again.");
return((int)States.GetCloseToTable);
}
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Can't reach location, will try to continue with the test.");
attemptCounter = 0;
if (objectTaken)
{
return((int)States.DropObject);
}
else
{
return((int)States.SearchAndTakeObject);
}
}
