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); }
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 InitialState(int currentState, object o) { brain.lastRecoGestures.Clear(); cmdMan.HEAD_lookat(0.0, -0.2, 5000); brain.SayAsync("I am looking for requests, please raise your hand above your head."); return((int)(States.SearchRequest)); }
int FinalState(int currentState, object o) { TextBoxStreamWriter.DefaultLog.WriteLine("head to 0,0"); cmdMan.HEAD_lookat(0, 0, 10000); this.finalState = FinalStates.OK; return(currentState); }
private int TrainShirt(int currentState, object o) { brain.SayAsync("Im going to remember your clothes."); cmdMan.HEAD_lookat(0.0, -0.3); Thread.Sleep(1000); TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Remembering shirt..."); brain.SayAsync("Human, please stand one meter in front of me."); Thread.Sleep(1000); if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_front", 3000)) { if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_front", 3000)) { cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_front", 3000); } } brain.SayAsync("Now, please turn to your left side."); Thread.Sleep(2000); if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_left", 3000)) { if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_left", 3000)) { cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_left", 3000); } } brain.SayAsync("Now, turn your back towards me."); Thread.Sleep(2000); if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_back", 3000)) { if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_back", 3000)) { cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_back", 3000); } } brain.SayAsync("Now, please turn to your right side."); Thread.Sleep(2000); if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_right", 3000)) { if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_right", 3000)) { cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_right", 3000); } } brain.SayAsync("Thank you."); Thread.Sleep(500); return((int)States.TrainFace); //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); }
private int SearchFall(int currentState, object o) { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> SearchFall state of SM_DetectPersonFall reached."); //double headAngle=-Math.PI/6; double headAngle = (-0.5 * 3.1416) / 180; cmdMan.HEAD_lookat(0.0, headAngle, 5000); //headAngle = (headAngle * 180) / Math.PI; headAngle = -0.5; brain.lastPersonFallDetected.Clear(); Thread.Sleep(5000); //enviar comando para que comienze a detectar caidas cmdMan.VISION_findfall(true, headAngle); timer.Start(); while (timer.Elapsed.Seconds < 25 && brain.lastPersonFallDetected.Count == 0) { ; } timer.Stop(); //enviar comando para que deje de detectar caidas cmdMan.VISION_findfall(false, 0); if (brain.lastPersonFallDetected.Count > 0) { fallDetected = true; } else { fallDetected = false; } if (!fallDetected) { finalState = FinalStates.FallNotDetected; return((int)States.FinalState); } return((int)States.GoToFall); }
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); }
private int PerformHipnotize(int currentState, object o) { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000-> PerformHypnotize state reached."); t_SPGENHypnoSpeech = new Thread(new ThreadStart(SPGENHypnoSpeech)); t_ARMSHypnoMovements = new Thread(new ThreadStart(ARMSHypnoMovements)); t_HEADHypnoMovements = new Thread(new ThreadStart(HEADHypnoMovements)); //to allow threads execution hypnoSpeechFinished = false; //introduction to hypno show cmdMan.SPG_GEN_shutup(1000); this.brain.SayAsync("I will try to hipnotize you."); Thread.Sleep(1000); this.brain.SayAsync("Look straight to my pendulum."); Thread.Sleep(1000); //move both arms to navigation moveActiveArms(SMConfiguration.ARMS_navigation, 10000); //move left arm to hypno moveOneArm(SMConfiguration.ARMS_hypnoc, 1, 10000); //Launch the thread to move asynchronous the robot's arms (hypno left and right) t_ARMSHypnoMovements.Start(); while (!t_ARMSHypnoMovements.IsAlive) { ; } //Launch the threads to move asynchronous the robot's head ("circular" movement) t_HEADHypnoMovements.Start(); while (!t_HEADHypnoMovements.IsAlive) { ; } //Launch the thread to asynchronously play the speech t_SPGENHypnoSpeech.Start(); while (!t_SPGENHypnoSpeech.IsAlive) { ; } if (t_SPGENHypnoSpeech.IsAlive) { t_SPGENHypnoSpeech.Join(); } if (t_HEADHypnoMovements.IsAlive) { t_HEADHypnoMovements.Join(); } if (t_ARMSHypnoMovements.IsAlive) { t_ARMSHypnoMovements.Join(); } //send the arms back to navigation and home moveActiveArms(SMConfiguration.ARMS_navigation, 10000); moveActiveArms(SMConfiguration.ARMS_home, 10000); //look at the human face cmdMan.HEAD_lookat(SMConfiguration.HEAD_lookToFace.pan, SMConfiguration.HEAD_lookToFace.tilt, 2000); this.brain.SayAsync("now you are under my control!!!"); Thread.Sleep(1000); this.brain.SayAsync("I command you to give me a token!"); Thread.Sleep(2000); return((int)States.InitialState); }