private int SearchWaving(int currentState, object o)
{
double xFall, zFall;
/*send command to find waving*/
//emergencyFound = false;
//if (!emergencyFound)
Thread.Sleep(1000);
if (!cmdMan.VISION_findwaving(headAngle, out xFall, out zFall, 10000))
{
finalState = FinalStates.WavingNotFound;
}
else
{
double distance, angle;
//double robotX, robotY, robotAngle;
//double goalX, goalY;
//this.cmdMan.MVN_PLN_position(out robotX, out robotY, out robotAngle, 1000);
distance = ((Math.Sqrt(Math.Pow(xFall, 2) + Math.Pow(zFall, 2))));
angle = Math.Atan2(-xFall, zFall);
//angle = 1.5708-robotAngle;
//goalX = robotX + (xFall * Math.Cos(angle) - zFall * Math.Sin(angle));
//goalY = robotY + (xFall * Math.Sin(angle) + zFall * Math.Cos(angle));
/*if(!cmdMan.MVN_PLN_getclose(goalX,goalY,10000))
* if (!cmdMan.MVN_PLN_getclose(goalX, goalY, 10000))
* cmdMan.MVN_PLN_getclose(goalX, goalY, 10000);
*/
if (!cmdMan.MVN_PLN_move((3 * distance) / 5, angle + headPan, 10000))
{
if (!cmdMan.MVN_PLN_move((3 * distance) / 5, angle + headPan, 10000))
{
cmdMan.MVN_PLN_move((3 * distance) / 5, angle + headPan, 10000);
}
}
//getclose to waving
finalState = FinalStates.OK;
}
return((int)States.FinalState);
}
int FinalState(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("head to 0,0");
cmdMan.HEAD_lookat(0, 0, 10000);
cmdMan.MVN_PLN_move(-0.4, 0.0, 10000);
return(currentState);
}
private int SearchRequest(int currentState, object o)
{
bool gestureFound = false;
for (int i = 0; i < 18; ++i)
{
Thread.Sleep(500);
if (gestureFound = brain.FindSpecificGesture("request", out requestId))
{
cmdMan.MVN_PLN_fixhuman(requestId, 4000);
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Gesture found.");
brain.SayAsync("I found a request.");
break;
}
}
if (!gestureFound)
{
if (attemptCounter > 7)
{
attemptCounter = 0;
finalState = FinalStates.RequestNotFound;
return((int)States.FinalState);
}
cmdMan.MVN_PLN_move(0, -Math.PI / 4, 4000);
if (attemptCounter % 2 == 1)
{
brain.SayAsync("I am looking for requests, please raise your hand above your head.");
}
attemptCounter++;
return(currentState);
}
// Si encontro humano, pasa al siguiente estado
attemptCounter = 0;
return((int)States.GoToRequest);
}
private int PerformPresentation(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000-> PerformPresentation state reached.");
//select the arm to use
int armToUse = -1; //0 for right, 1 for left, others numbers for any arm
//introduction
this.cmdMan.SPG_GEN_say("Hello. I am the Robot Justina and I was built in the National University of Mexico", 15000);
this.cmdMan.SPG_GEN_say("I would like to show you my design", 10000);
moveOneArm("heilHitler", armToUse, 10000);
//head show
this.cmdMan.SPG_GEN_say("I have a mecha-tronic head", 10000);
moveOneArm("showHead", armToUse, 10000);
this.cmdMan.HEAD_lookat(0.7, 0.0, 10000);
this.cmdMan.HEAD_lookat(0.0, 0.5, 10000);
this.cmdMan.HEAD_lookat(-0.7, 0.0, 10000);
this.cmdMan.HEAD_lookat(0.0, -0.5, 10000);
this.cmdMan.HEAD_lookat(0, 0, 10000);
moveOneArm("heilHitler", armToUse, 10000);
//kinect show
this.cmdMan.SPG_GEN_say("I have a kinect system", 10000);
moveOneArm("heilHitler", armToUse, 10000);
Thread.Sleep(1000);
//arms show
moveActiveArms("showArm", 12000);
this.cmdMan.SPG_GEN_say("I have two arms", 2000);
this.cmdMan.SPG_GEN_say("These are an anthropomophic seven degree of freedom manipulators", 2000);
Thread.Sleep(1000);
moveActiveArms("home", 12000);
//laser show
this.cmdMan.SPG_GEN_say("I have a laser sensor", 2000);
moveOneArm("showLaser", armToUse, 10000);
Thread.Sleep(1000);
moveActiveArms("home", 12000);
//outting
this.cmdMan.SPG_GEN_say("Finally, I have a differential pair mobile base for navigating", 15000);
cmdMan.MVN_PLN_move(0, Math.PI / 4, 4000);
cmdMan.MVN_PLN_move(0, -Math.PI / 4, 4000);
this.cmdMan.SPG_GEN_say("Thank you very much for your attention");
return((int)States.InitialState);
}
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);
}
/// <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);
}
