/// <summary>
/// The robot enters to the arena an goes to the MVN_PLN EntranceLocation
/// </summary>
private int EnterArena(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> State reached EnterArena");
SM_EnterArena.FinalStates enterArena_status;
SM_EnterArena enterArena = new SM_EnterArena(brain, cmdMan, SMConfiguration.EntranceLocation);
enterArena_status = enterArena.Execute();
if (enterArena_status == SM_EnterArena.FinalStates.OK)
{
if (!cmdMan.ARMS_goto(SMConfiguration.ArmsNavigationPosition, 3000))
{
if (!cmdMan.ARMS_goto(SMConfiguration.ArmsNavigationPosition, 3000))
{
cmdMan.ARMS_goto(SMConfiguration.ArmsNavigationPosition, 3000);
}
}
return((int)States.GetCloseToTable);
}
else
{
return((int)States.EnterArena);
}
}
private int Navigate_Succeeded(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Navigate_Succeeded state reached.");
//return the arms to home position
if (!(SMConfiguration.ARMS_usedArm == ""))
{
cmdMan.ARMS_goto(SMConfiguration.ARMS_home, 10000);
}
cmdMan.SPG_GEN_say(navigSucceded, 3000);
finalStatus = Status.OK;
// TODO: Change the next status
return((int)States.FinalState);
}
/// <summary>
/// The robot start to follow a human (guide phase).
/// </summary>
private int FollowHuman(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> FollowHuman state reached.");
//start saving the robot's path (hansel & gretel)
cmdMan.MVN_PLN_startsavingpath();
cmdMan.HEAD_lookat(SMConfiguration.HeadHomePan, SMConfiguration.HeadHomeTilt, 10000);
//Thread.Sleep(500);
//cmdMan.HEAD_lookat(SMConfiguration.HeadFollowPan, SMConfiguration.HeadFollowTilt, 10000);
//send the arms to a position above the laser
cmdMan.ARMS_goto(SMConfiguration.ArmsFollowPosition);
brain.SayAsync(SMConfiguration.StartFollowMessage);
//start to follow a human
cmdMan.MVN_PLN_startfollowhuman();
brain.recognizedSentences.Clear();
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Waiting for location command");
return((int)States.WaitForLocationCommand);
}
