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); }