private bool tryNavigationToTable(string locationToReach)
{
bool success = false;
if (!brain.GetCloseToTable(locationToReach, 50000))
{
if (!brain.GetCloseToTable(locationToReach, 50000))
{
if (brain.GetCloseToTable(locationToReach, 50000))
{
success = true;
}
}
else
{
success = true;
}
}
else
{
success = true;
}
return(success);
}
/// <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);
}
}
private int GoToDrinksLocation(int currentState, object o)
{
cmdMan.HEAD_lookat(0, 0, 10000);
TextBoxStreamWriter.DefaultLog.WriteLine("Going to drinks location.");
this.cmdMan.ARMS_goto("standby", 8000);
if (brain.GetCloseToTable(drinksLocation, 180000))
{
brain.SayAsync("i have arrived to the drinks location");
this.attemptCounter = 0;
return((int)States.SearchAndTakeObject);
}
if (attemptCounter < 3)
{
attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("Cant reach drinks location, trying again");
return(currentState);
}
TextBoxStreamWriter.DefaultLog.WriteLine("Cant reach drinks location, will try to continue with the test");
return((int)States.SearchAndTakeObject);
}