private void RemoveObjectsInvolvingState(string stateName) { List <Instruction> temp = new List <Instruction>(); // Remove related instructions foreach (Instruction instruction in Instructions) { if (instruction.CurrentState == stateName || instruction.NextState == stateName) { temp.Add(instruction); } } foreach (Instruction instruction in temp) { Instructions.Remove(instruction); } // Remove related final states if (FinalStates.Contains(stateName)) { FinalStates.Remove(stateName); } // Remove related initial state RemoveInitialStateInvolvingState(stateName); }
private void ReduceStates() { var newState = 'A'; for (int i = 0; i < States.Count; i++) { if (StartState == States[i]) { StartState = newState.ToString(); } if (FinalStates.Contains(States[i])) { FinalStates[FinalStates.IndexOf(States[i])] = newState.ToString(); } foreach (var tr in Transitions) { if (tr.CurrentState == States[i]) { tr.CurrentState = newState.ToString(); } if (tr.NextState == States[i]) { tr.NextState = newState.ToString(); } } States[i] = newState.ToString(); newState++; } }
/// <summary> /// Recursively checks to see if we can form the string /// </summary> /// <param name="state">the state to check</param> /// <param name="remainingString">the remainder of the string to check</param> /// <returns>A boolean that is true if the string has been fully formed</returns> private bool CheckNextNodeDfa(T state, string stringToVerify) { bool stringIsAccepted = false; if (stringToVerify == string.Empty) //Early escape if we're finished { stringIsAccepted = FinalStates.Contains(state); return(stringIsAccepted); } char currentCharacter = stringToVerify[0]; if (stringToVerify.Count() > 1) { stringToVerify = stringToVerify.Substring(1); } else { stringToVerify = string.Empty; } IEnumerable <Transition <T> > validTransitions = Transitions.Where(x => x.Identifier == currentCharacter && x.FromState.Equals(state)); foreach (Transition <T> transition in validTransitions) { stringIsAccepted = CheckNextNodeDfa(transition.ToState, stringToVerify); if (stringIsAccepted) { break; } } return(stringIsAccepted); }
/// <summary> /// Adds a letter to a language, then returns the string /// </summary> /// <param name="remainingLength">The remaining length for the string</param> /// <param name="stringSoFar">The string generated so far.</param> /// <param name="state">The state from which to generate a letter.</param> /// <returns>Either returns a valid string, or an empty string if there is a non valid string.</returns> private string AddLetterToLanguage(int remainingLength, string stringSoFar, T state) { string resultingString = string.Empty; if (remainingLength > 0) { remainingLength = remainingLength - 1; IEnumerable <Transition <T> > validTransitions = Transitions.Where(x => x.FromState.Equals(state)); foreach (Transition <T> transition in validTransitions) { string newString = string.Concat(stringSoFar, transition.Identifier); resultingString = AddLetterToLanguage(remainingLength, newString, transition.ToState); if (string.IsNullOrEmpty(resultingString)) { continue; } else { break; } } } else { resultingString = FinalStates.Contains(state) ? stringSoFar : string.Empty; } return(resultingString); }
public SM_SearchAndTakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string ObjectToFind, bool useHandOver) { SM = new FunctionBasedStateMachine(); this.brain = brain; this.cmdMan = cmdMan; this.attemptCounter = 0; this.searchAttemptCounter = 0; this.finalState = FinalStates.StillRunning; this.objectsFound = new List<string>(); this.useTakeHandOver = useHandOver; this.anyObject = anyObject; if (anyObject) this.ObjectToFind = "objects"; else this.ObjectToFind = ObjectToFind; this.foundObject = ObjectToFind; SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects))); SM.AddState(new FunctionState((int)States.SetObjectToTake, new SMStateFuncion(SetObjectToTake))); SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState))); SM.SetFinalState((int)States.FinalState); }
public SSA <SYMBOL> Collapse(Func <SSA <SYMBOL>, int, int, bool> statesAreEquivalent) { /* map state to equivalency class representative */ var stateMap = new Dictionary <int, int>(); foreach (int state in States) { foreach (int repr in stateMap.Values.Distinct()) // search equivalency classes { if (statesAreEquivalent(this, state, repr)) { stateMap[state] = repr; // add to equivalency class break; } } if (!stateMap.ContainsKey(state)) // equivalency class not found { stateMap[state] = state; // new equivalency class } } int initialState = stateMap[InitialState]; var finalStates = new Set <int>(FinalStates.Select(fs => stateMap[fs])); var moves = new Set <Move <Predicate <SYMBOL> > >(Moves.Select(move => new Move <Predicate <SYMBOL> >(stateMap[move.SourceState], stateMap[move.TargetState], move.Label) )); var alphabet = new Set <SYMBOL>(Alphabet); return(new SSA <SYMBOL>(initialState, finalStates, moves, alphabet)); }
public SM_SearchAndTakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string ObjectToFind, bool useHandOver) { SM = new FunctionBasedStateMachine(); this.brain = brain; this.cmdMan = cmdMan; this.attemptCounter = 0; this.searchAttemptCounter = 0; this.finalState = FinalStates.StillRunning; this.objectsFound = new List <string>(); this.useTakeHandOver = useHandOver; this.anyObject = anyObject; if (anyObject) { this.ObjectToFind = "objects"; } else { this.ObjectToFind = ObjectToFind; } this.foundObject = ObjectToFind; SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects))); SM.AddState(new FunctionState((int)States.SetObjectToTake, new SMStateFuncion(SetObjectToTake))); SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState))); SM.SetFinalState((int)States.FinalState); }
int SearchCloseObjects(int currentState, object o) { if (cmdMan.ST_PLN_fashionfind_object(ObjectToFind, 180000, out objectsFound)) { attemptCounter = 0; TextBoxStreamWriter.DefaultLog.WriteLine("Objects found."); return((int)States.SetObjectToTake); } if (attemptCounter < 2) { attemptCounter++; TextBoxStreamWriter.DefaultLog.WriteLine("No objects were found, trying again"); this.brain.SayAsync("I did not find the " + ObjectToFind + ". I will try again"); //this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000); Thread.Sleep(2500); return(currentState); } this.brain.SayAsync("I did not find " + (anyObject?"any":"") + ObjectToFind); TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, SM was not successful."); attemptCounter = 0; this.finalState = FinalStates.Failed; return((int)States.FinalState); }
public bool TestInput(string input) { Log += ConsoleWriter.Info("Trying to parse: " + input) + "\n"; if (InvalidInput(input)) { return(false); } var currentState = StartState; var steps = new StringBuilder(); foreach (var symbol in input.ToCharArray()) { var transitions = TransitionFunctions; var transition = transitions.Find(t => t.InputState == currentState && t.InputSymbol == symbol); if (transition == null) { Log += ConsoleWriter.Failure("No transitions for current state and symbol") + "\n"; Log += ConsoleWriter.Failure(steps.ToString()) + "\n"; return(false); } currentState = transition.OutputState; steps.Append(transition + "\n"); } if (FinalStates.Contains(currentState)) { Log += ConsoleWriter.Success("Accepted the input with steps:\n" + steps) + "\n"; return(true); } Log += ConsoleWriter.Failure("Stopped in state " + currentState + " which is not a final state.") + "\n"; Log += ConsoleWriter.Failure(steps.ToString()) + "\n"; return(false); }
public SM_TakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string ObjectToFind, string objectLocation, bool succesGetClose) { SM = new FunctionBasedStateMachine(); this.brain = brain; this.cmdMan = cmdMan; this.objectLocation = objectLocation; attemptCounter = 0; this.finalState = FinalStates.StillRunning; this.anyObject = anyObject; if (anyObject) this.ObjectToFind = ObjectToFind; else this.ObjectToFind = "objects"; this.foundObject = ObjectToFind; this.succesGetClose = succesGetClose; SM.AddState(new FunctionState((int)States.GetCloseObjectLocation, new SMStateFuncion(GetCloseObjectLocation))); SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects))); SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState))); SM.SetFinalState((int)States.FinalState); }
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 GoToFall(int currentState, object o) { bool notHumanFall; TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> GoToFall state of SM_DetectPersonFall reached."); if (brain.GetCloseToHumanFall(out notHumanFall, 10000)) { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human success."); finalState = FinalStates.OK; return((int)States.FinalState); } else { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human FAILED"); if (!notHumanFall) { finalState = FinalStates.FallNotReached; } else { //brain.SayAsync("I have detected a fall in that direction."); //point at the direction of the fall //Thread.Sleep(1000); finalState = FinalStates.NoHumanFallDetected; } return((int)States.FinalState); } }
public SM_TakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string ObjectToFind, string objectLocation, bool succesGetClose) { SM = new FunctionBasedStateMachine(); this.brain = brain; this.cmdMan = cmdMan; this.objectLocation = objectLocation; attemptCounter = 0; this.finalState = FinalStates.StillRunning; this.anyObject = anyObject; if (anyObject) { this.ObjectToFind = ObjectToFind; } else { this.ObjectToFind = "objects"; } this.foundObject = ObjectToFind; this.succesGetClose = succesGetClose; SM.AddState(new FunctionState((int)States.GetCloseObjectLocation, new SMStateFuncion(GetCloseObjectLocation))); SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects))); SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState))); SM.SetFinalState((int)States.FinalState); }
private Grammar MakeGrammarLeft() { IDictionary <char, TerminalSymbol> charTerminalMap = Alphabet.ToDictionary(c => c, c => new TerminalSymbol(c)); IDictionary <Label, NonTerminalSymbol> stateNonTerminalMap = States.ToDictionary(s => s, s => new NonTerminalSymbol(s)); NonTerminalSymbol target = Grammar.GetNewNonTerminal(stateNonTerminalMap.Values); ISet <Rule> rules = new HashSet <Rule>(); foreach (Transition transition in Transitions) { Chain chain = new Chain( EnumerateHelper.Sequence <Symbol>( stateNonTerminalMap[transition.CurrentState], charTerminalMap[transition.Symbol] ) ); rules.Add(new Rule(chain.AsSequence(), stateNonTerminalMap[transition.NextState])); } rules.Add(new Rule(Chain.Empty.AsSequence(), stateNonTerminalMap[InitialState])); IEnumerable <Chain> chains = FinalStates.Select(fs => new Chain(stateNonTerminalMap[fs].AsSequence())); rules.Add(new Rule(chains, target)); return(new Grammar(rules, target)); }
public void RemoveState(ail.net.parser.FsaState xi_state) { ail.net.framework.Assert.NonNullReference(xi_state, "xi_state"); ail.net.framework.Assert.Condition(States.Contains(xi_state.Id), "States.Contains(xi_state.Id)"); FinalStates.Remove(xi_state.Id); States.Remove(xi_state.Id); }
/// <summary> /// Add state to finalstates /// </summary> /// <param name="state">The state</param> public void DefineAsFinalState(T state) { if (!DfaContainsState(state)) { States.Add(state); } FinalStates.Add(state); }
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); }
public void DefineAsFinalState(T t) { if (!States.Contains(t)) { States.Add(t); } FinalStates.Add(t); }
public SM_SearchAndTakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string [] objectsToFind, bool useHandOver, int searchAttempts = 2, bool twoArms = false) { SM = new FunctionBasedStateMachine(); this.brain = brain; this.cmdMan = cmdMan; this.attemptCounter = 0; this.searchAttemptCounter = 0; this.searchAttempts = searchAttempts; this.finalState = FinalStates.StillRunning; this.twoArms = twoArms; this.takeCounter = 0; this.firstObjectTaken = false; /**********************/ this.laTaken = false; this.armToUse = ""; this.objectsFound = new List <string>(); this.useTakeHandOver = useHandOver; this.objectsToFind = new string[2]; this.foundObjects = new string[2]; this.armsOrder = new string[2]; this.foundObjects[0] = ""; this.foundObjects[1] = ""; this.armsOrder[0] = ""; this.armsOrder[1] = ""; this.anyObject = anyObject; if (anyObject) { //this.ObjectToFind = "objects"; this.objectsToFind[0] = "objects"; this.objectsToFind[1] = ""; } else { this.objectsToFind[0] = objectsToFind[0]; this.objectsToFind[1] = objectsToFind[1]; } //this.objectsToTake = new string[2]; //this.objectsToTake[0] = ""; //this.objectsToTake[1] = ""; //this.foundObjects = new string[2]; //this.foundObjects[0] = ObjectToFind; //this.foundObjects[1] = ""; //objectsToTake = new string[2]; SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects))); SM.AddState(new FunctionState((int)States.SetObjectToTake, new SMStateFuncion(SetObjectToTake))); SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState))); SM.SetFinalState((int)States.FinalState); }
public bool RemoveFinalState(string stateName) { if (FinalStates.Contains(stateName)) { FinalStates.Remove(stateName); return(true); } return(false); }
public bool AddFinalState(string stateName) { if (States.Contains(stateName) && !FinalStates.Contains(stateName)) { FinalStates.Add(stateName); return(true); } return(false); }
public SM_SearchAndTakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string [] objectsToFind, bool useHandOver, int searchAttempts = 2, bool twoArms=false) { SM = new FunctionBasedStateMachine(); this.brain = brain; this.cmdMan = cmdMan; this.attemptCounter = 0; this.searchAttemptCounter = 0; this.searchAttempts = searchAttempts; this.finalState = FinalStates.StillRunning; this.twoArms = twoArms; this.takeCounter = 0; this.firstObjectTaken=false; /**********************/ this.laTaken = false; this.armToUse = ""; this.objectsFound = new List<string>(); this.useTakeHandOver = useHandOver; this.objectsToFind = new string[2]; this.foundObjects = new string[2]; this.armsOrder = new string[2]; this.foundObjects[0] = ""; this.foundObjects[1] = ""; this.armsOrder[0] = ""; this.armsOrder[1] = ""; this.anyObject = anyObject; if (anyObject) { //this.ObjectToFind = "objects"; this.objectsToFind[0] = "objects"; this.objectsToFind[1] = ""; } else { this.objectsToFind[0] = objectsToFind[0]; this.objectsToFind[1] = objectsToFind[1]; } //this.objectsToTake = new string[2]; //this.objectsToTake[0] = ""; //this.objectsToTake[1] = ""; //this.foundObjects = new string[2]; //this.foundObjects[0] = ObjectToFind; //this.foundObjects[1] = ""; //objectsToTake = new string[2]; SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects))); SM.AddState(new FunctionState((int)States.SetObjectToTake, new SMStateFuncion(SetObjectToTake))); SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState))); SM.SetFinalState((int)States.FinalState); }
public bool IsWordInLanguage(String input, State startState) { //if we can finish now, finish now if (input.Length == 0) { return(FinalStates.Contains(startState)); } return(GetStatesAccessibleFrom(startState, input).Intersect(FinalStates).Count() > 0); }
private int PersiguiendoPersona(int currentState, object o) { double distanc = Math.Sqrt(Math.Pow(hum.X, 2) + Math.Pow(hum.Y, 2)); if (distanc >= 0.5) { distanc = distanc - 0.10; } else if (distanc >= 0.3 && distanc < 0.5) { distanc = 0; } else if (distanc < 0.3) { distanc = -1; } double ang = 0; if (hum.X == 0) { if (hum.Y > 0) { ang = Math.PI / 2; } else { ang = -Math.PI / 2; } } else { ang = Math.Atan2(hum.Y, hum.X); if (Math.Abs(ang) < AnguloEstable) { ang = 0; } } if (distanc < 0) { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> por fin"); finalState = FinalStates.HumanReached; return((int)States.AntesDeTerminar); } else if (distanc == 0) { return((int)States.SensandoPersona); } else { this.cmdMan.MVN_PLN_move(distanc, ang); TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> siguiendo a la persona"); return((int)States.SensandoPersona); } }
public void AddFinalState(ail.net.parser.FsaState xi_state, ail.net.parser.Token xi_token) { ail.net.framework.Assert.NonNullReference(xi_state, "xi_state"); ail.net.framework.Assert.NonNullReference(xi_token, "xi_token"); if (!FinalStates.Contains(xi_state.Id)) { xi_state.Token = xi_token; FinalStates.Add(xi_state.Id, xi_state); } }
private int TrainFace(int currentState, object o) { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Sending <remember_human> command"); this.cmdMan.MVN_PLN_position(out x, out y, out angle, 2000); string person; if (this.cmdMan.ST_PLN_findhuman("human", "hdtilt hdpan", 90000, out person)) { if (this.cmdMan.ST_PLN_rememberhuman(foundHuman, "", 60000)) { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Face of " + foundHuman + " remembered succesfully"); brain.SayAsync("I have remembered your face."); Thread.Sleep(500); } else { if (attemptCounter < 2) { attemptCounter++; return(currentState); } else { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Cannot remember the face of " + foundHuman); //brain.SayAsync("I cannot remember your face. I will continue with the test"); //return (int)States.TrainShirt; this.finalState = FinalStates.HumanNotLearned; return((int)States.FinalState); } } } else { if (attemptCounter < 2) { attemptCounter++; return(currentState); } else { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Cannot find human."); this.finalState = FinalStates.HumanNotFound; //return (int)States.TrainShirt; return((int)States.FinalState); } } attemptCounter = 0; TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumandRoutine.-> Desactivating reco human"); this.cmdMan.PRS_FND_sleep(true); this.finalState = FinalStates.OK; return((int)States.FinalState); }
public KeyValuePair <bool, int> MaxString(string str, int pos) { KeyValuePair <bool, int> MaxStr = new KeyValuePair <bool, int>(); int m = 0; bool res = false; int curState = TakeNewStartState(str[pos]); if (FinalStates.Contains(curState)) { res = true; } int i = pos; while (i < str.Length) { if (IsStateTransitionSignalsContainsSignal(curState, str[i])) { foreach (var state in States) { if (state.NameOfState == curState) { int newState = 0; foreach (var list in state.StateTransitionSignals) { foreach (var elem in list) { if (elem == str[i]) { curState = state.AvailableStates[newState]; } } newState++; } } } if (FinalStates.Contains(curState)) { m = i - pos + 1; res = true; MaxStr = new KeyValuePair <bool, int>(res, m); } i++; } else { return(MaxStr); } } return(MaxStr); }
public StateMachine Reorganize(IDictionary <Label, Label> map) { if (map == null) { throw new ArgumentNullException(nameof(map)); } Label initialState = map[InitialState]; IEnumerable <Label> finalStates = FinalStates.Select(fs => map[fs]); IEnumerable <Transition> transitions = Transitions.Select(t => new Transition(map[t.CurrentState], t.Symbol, map[t.NextState])); return(new StateMachine(initialState, finalStates, transitions)); }
/// <summary> /// Recursively checks to see if we can form the string /// </summary> /// <param name="givenTransition">the transition to check</param> /// <param name="remainingString">the remainder of the string to check</param> /// <returns>A boolean that is true if the string has been fully formed</returns> private bool CheckNextNodeNdfa(Transition <T> givenTransition, string stringToVerify) { bool stringIsAccepted = false; if (stringToVerify == string.Empty) //Early escape if we're finished { stringIsAccepted = FinalStates.Contains(givenTransition.ToState); if (stringIsAccepted) { return(stringIsAccepted); } IEnumerable <Transition <T> > possibleTransitionsLeadingToEndState = Transitions.Where(x => x.Identifier == '$' && x.FromState.Equals(givenTransition.ToState)); stringIsAccepted = CheckEmptyTransitions(possibleTransitionsLeadingToEndState); return(stringIsAccepted); } char currentCharacter = stringToVerify[0]; if (stringToVerify.Count() > 1) { stringToVerify = stringToVerify.Substring(1); } else { stringToVerify = string.Empty; } IEnumerable <Transition <T> > validTransitions = Transitions.Where(x => (x.Identifier == currentCharacter || x.Identifier == '$') && x.FromState.Equals(givenTransition.ToState)); foreach (Transition <T> transition in validTransitions) { if (transition.Identifier != '$') { stringIsAccepted = CheckNextNodeNdfa(transition, stringToVerify); } else { stringIsAccepted = CheckNextNodeNdfa(transition, currentCharacter + stringToVerify); } if (stringIsAccepted) { break; } } return(stringIsAccepted); }
int SetObjectToTake(int currentState, object o) { objectFoundIndex = 0; this.foundObjectsCount = (byte)objectsFound.Count; if (this.foundObjectsCount == 0) { if (!firstObjectTaken) { TextBoxStreamWriter.DefaultLog.WriteLine("No reachable objects were found, SM was not successful."); this.finalState = FinalStates.Failed; return((int)States.FinalState); } else { this.finalState = FinalStates.OK; return((int)States.FinalState); } } if (!anyObject) { if (objectsFound.Contains(objectsToFind[takeCounter])) { objectFoundIndex = objectsFound.IndexOf(objectsToFind[takeCounter]); TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object found."); return((int)States.TakeObject); } ///******28-05-2013*******//////////// if (searchAttemptCounter < searchAttempts) { searchAttemptCounter++; TextBoxStreamWriter.DefaultLog.WriteLine("object NOT found, trying again"); this.brain.SayAsync("I did not find the " + objectsToFind[takeCounter] + ". I will try again"); //this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000); Thread.Sleep(2500); return((int)States.SearchCloseObjects); } TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, test failed."); this.finalState = FinalStates.Failed; return((int)States.FinalState); } objectsToFind[takeCounter] = objectsFound[0]; attemptCounter = 0; TextBoxStreamWriter.DefaultLog.WriteLine("Some Object found."); return((int)States.TakeObject); }
/// <summary> /// Checks whether the given string fits within this automata /// </summary> /// <param name="stringToVerify">The string to check.</param> /// <returns>A boolean indicator that is true if the string can be formed by this automata by traversing it's nodes.</returns> private bool IsStringAcceptableNdfa(string stringToVerify) { bool stringIsAccepted = false; //Pak start states + eerste element in char array char charToCheckFor = stringToVerify[0]; HashSet <T> startingStates = StartStates; //Verwijder eerste character if (stringToVerify.Count() > 1) { stringToVerify = stringToVerify.Substring(1); } else { stringToVerify = string.Empty; } //Voor iedere uitgaande transitie check of het kan met het gegeven character foreach (T state in startingStates) { if (stringToVerify == string.Empty) { stringIsAccepted = FinalStates.Contains(state); break; } IEnumerable <Transition <T> > validTransitions = Transitions.Where(x => (x.Identifier == charToCheckFor || x.Identifier == '$') && x.FromState.Equals(state)); foreach (Transition <T> transition in validTransitions) { if (transition.Identifier != '$') { stringIsAccepted = CheckNextNodeNdfa(transition, stringToVerify); } else { stringIsAccepted = CheckNextNodeNdfa(transition, charToCheckFor + stringToVerify); } if (stringIsAccepted) { break; } } } //Als er een terug komt met true, dan bestaat de string return(stringIsAccepted); }
/// <summary> /// Creates a state machine to enter the arena up to a location known by the motion planner. /// </summary> /// <param name="brain">A HAL9000Brain instance</param> /// <param name="location">A location known by the motion planner where the robot should try to go when entering the arena.</param> public SM_WaitForRequest(HAL9000Brain brain, HAL9000CmdMan cmdMan) { this.brain = brain; this.cmdMan = cmdMan; this.attemptCounter = 0; SM = new FunctionBasedStateMachine(); SM.AddState(new FunctionState((int)States.InitialState, new SMStateFuncion(InitialState))); SM.AddState(new FunctionState((int)States.SearchRequest, new SMStateFuncion(SearchRequest))); SM.AddState(new FunctionState((int)States.GoToRequest, new SMStateFuncion(GoToRequest))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState), true)); SM.SetFinalState((int)States.FinalState); finalState = FinalStates.StillRunning; }
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); }
/// <summary> /// Creates a state machine to enter the arena up to a location known by the motion planner. /// </summary> /// <param name="brain">A HAL9000Brain instance</param> /// <param name="location">A location known by the motion planner where the robot should try to go when entering the arena.</param> /// <param name="tryToOpenDoor">A boolean indicating whether the robot should try to open the door on its own or ask for help directly.</param> public SM_EnterArena(HAL9000Brain brain, HAL9000CmdMan cmdMan, string location, bool tryToOpenDoor) { this.brain = brain; this.cmdMan = cmdMan; this.location = location; this.finalState = FinalStates.StillRunning; this.attemptCounter = 0; SM = new FunctionBasedStateMachine(); SM.AddState(new FunctionState((int)States.InitialState, new SMStateFuncion(InitialState))); SM.AddState(new FunctionState((int)States.WaitForDoorToBeOpened, new SMStateFuncion(WaitForDoorToBeOpened))); SM.AddState(new FunctionState((int)States.EnterArena, new SMStateFuncion(EnterArena))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState), true)); SM.SetFinalState((int)States.FinalState); }
/// <summary> /// Creates a state machine to enter the arena up to a location known by the motion planner. /// </summary> /// <param name="brain">A HAL9000Brain instance</param> /// <param name="location">A location known by the motion planner where the robot should try to go when entering the arena.</param> public SM_DetectPersonFall(HAL9000Brain brain, HAL9000CmdMan cmdMan) { this.brain = brain; this.cmdMan = cmdMan; timer = new System.Diagnostics.Stopwatch(); fallDetected = false; SM = new FunctionBasedStateMachine(); SM.AddState(new FunctionState((int)States.InitialState, new SMStateFuncion(InitialState))); SM.AddState(new FunctionState((int)States.SearchFall, new SMStateFuncion(SearchFall))); SM.AddState(new FunctionState((int)States.GoToFall, new SMStateFuncion(GoToFall))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState), true)); SM.SetFinalState((int)States.FinalState); finalState = FinalStates.StillRunning; }
/// <summary> /// Creates a state machine to enter the arena up to a location known by the motion planner. /// </summary> /// <param name="brain">A HAL9000Brain instance</param> /// <param name="location">A location known by the motion planner where the robot should try to go when entering the arena.</param> public SM_DetectWaving(HAL9000Brain brain, HAL9000CmdMan cmdMan, double headAngle, double headPan) { this.brain = brain; this.cmdMan = cmdMan; this.headAngle = headAngle; this.headPan = headPan; SM = new FunctionBasedStateMachine(); SM.AddState(new FunctionState((int)States.InitialState, new SMStateFuncion(InitialState))); SM.AddState(new FunctionState((int)States.SearchWaving, new SMStateFuncion(SearchWaving))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState), true)); SM.SetFinalState((int)States.FinalState); finalState = FinalStates.StillRunning; }
public SM_FollowHuman(HAL9000Brain brain, HAL9000CmdMan cmdman) { this.brain = brain; this.cmdMan = cmdman; TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Ejecutando prueba de seguimiento"); this.brain.Status.TestBeingExecuted = "Fllw1"; this.brain.OnStatusChanged(new HAL9000StatusArgs(this.brain.Status)); this.finalState = FinalStates.StillRunning; SM = new FunctionBasedStateMachine(); SM.AddState(new FunctionState((int)States.SubiendoMisBrazos, SubiendoMisBrazos)); SM.AddState(new FunctionState((int)States.EsperandoPersonaEnFrente, EsperandoPersonaEnFrente)); SM.AddState(new FunctionState((int)States.SensandoPersona, SensandoPersona)); SM.AddState(new FunctionState((int)States.PersiguiendoPersona, PersiguiendoPersona)); SM.AddState(new FunctionState((int)States.AntesDeTerminar, AntesDeTerminar, true)); SM.AddState(new FunctionState((int)States.FinalState, FinalState, true)); SM.SetFinalState((int)States.FinalState); }
public SM_TakeTray(HAL9000Brain brain, HAL9000CmdMan cmdMan, double height) { SM = new FunctionBasedStateMachine(); this.brain = brain; this.cmdMan = cmdMan; distance = 0; this.height = height; headPan = 0; headTilt = -1.1; percent = 50; this.finalState = FinalStates.StillRunning; SM.AddState(new FunctionState((int)States.LineTable,LineTable)); SM.AddState(new FunctionState((int)States.TakeTray, TakeTray)); SM.AddState(new FunctionState((int)States.FinalState, FinalState,true)); SM.SetFinalState((int)States.FinalState); }
/// <summary> /// Creates a state machine to enter the arena up to a location known by the motion planner. /// </summary> /// <param name="brain">A HAL9000Brain instance</param> /// <param name="location">A location known by the motion planner where the robot should try to go when entering the arena.</param> public SM_SimpleIdentifyPerson(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool startIdentifyPerson, string[] knownPersons, string defaultName, List<string> rejectedNames) { this.brain = brain; this.cmdMan = cmdMan; this.startIdentifyPerson = startIdentifyPerson; this.defaultName = defaultName; this.knownPersons = knownPersons; this.rejectedNames = rejectedNames; SM = new FunctionBasedStateMachine(); SM.AddState(new FunctionState((int)States.InitialState, new SMStateFuncion(InitialState))); SM.AddState(new FunctionState((int)States.SpeachToHuman, new SMStateFuncion(SpeachToHuman))); SM.AddState(new FunctionState((int)States.FindHuman, new SMStateFuncion(FindHuman))); SM.AddState(new FunctionState((int)States.AskForName, new SMStateFuncion(AskForName))); SM.AddState(new FunctionState((int)States.AsociateName, new SMStateFuncion(AsociateName))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState), true)); SM.SetFinalState((int)States.FinalState); finalState = FinalStates.StillRunning; }
public CocktailPartyTest(HAL9000Brain brain, HAL9000CmdMan cmdMan) { this.brain = brain; this.cmdMan = cmdMan; TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Executing Cocktail Party Test"); this.brain.Status.TestBeingExecuted = "Cocktail Party"; this.brain.OnStatusChanged(new HAL9000StatusArgs(this.brain.Status)); this.finalState = FinalStates.StillRunning; tryToOpenDoor = false; entranceLocation = "frontentrance"; partyroomLocation = "partyroom"; drinksLocation = "drinks"; exitLocation = "exit"; defaultDrink = "mangojuice"; attemptCounter = 0; gestureFailed = false; defaultNames = new string[] { "Alan", "Albert", "Angel" }; SM = new FunctionBasedStateMachine(); //orders = new Queue<order>(3); ordersCount = 0; rejectedNames = new List<string>(3); SM.AddState(new FunctionState((int)States.EnterArena, new SMStateFuncion(EnterArena))); SM.AddState(new FunctionState((int)States.GoToPartyRoom, new SMStateFuncion(GoToPartyRoom))); SM.AddState(new FunctionState((int)States.WaitForRequest, new SMStateFuncion(WaitForRequest))); SM.AddState(new FunctionState((int)States.IdentifyPerson, new SMStateFuncion(IdentifyPerson))); SM.AddState(new FunctionState((int)States.WaitForOrder, new SMStateFuncion(WaitForOrder))); SM.AddState(new FunctionState((int)States.NoGestureRecognized, new SMStateFuncion(NoGestureRecognized))); SM.AddState(new FunctionState((int)States.WaitForConfirmation, new SMStateFuncion(WaitForConfirmation))); SM.AddState(new FunctionState((int)States.EnqueueOrder, new SMStateFuncion(EnqueueOrder))); SM.AddState(new FunctionState((int)States.GoToDrinksLocation, new SMStateFuncion(GoToDrinksLocation))); SM.AddState(new FunctionState((int)States.SearchAndTakeObject, new SMStateFuncion(SearchAndTakeObject))); SM.AddState(new FunctionState((int)States.GoToHumanLocation, new SMStateFuncion(GoToHumanLocation))); SM.AddState(new FunctionState((int)States.DeliverDrink, new SMStateFuncion(DeliverDrink))); SM.AddState(new FunctionState((int)States.LeaveArena, new SMStateFuncion(LeaveArena))); SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState))); SM.SetFinalState((int)States.FinalState); }
public OpenChallenge_Training(HAL9000Brain brain, HAL9000CmdMan cmdMan) { this.brain = brain; this.cmdMan = cmdMan; this.brain.UseFloorLaser = false; height = 0; LocationTable = "table"; LocationTray = "tray"; percent = 50; distance = 0; headPan = 0; headTilt = -1.1; voiceRecognitionAttempts = 0; alreadyKnownPersons = new List<string>(); defaultNames = new string[] {"John", "James", "William", "Michael"}; personCounter = 0; usingRightArm = true; TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Executing Open Challenge"); this.brain.Status.TestBeingExecuted = "Open Challenge"; this.brain.OnStatusChanged(new HAL9000StatusArgs(this.brain.Status)); this.finalState = FinalStates.StillRunning; SM = new FunctionBasedStateMachine(); SM.AddState(new FunctionState((int)States.CallRobot, CallRobot)); SM.AddState(new FunctionState((int)States.TrainFace, TrainFace)); SM.AddState(new FunctionState((int)States.DeliverSheet, DeliverSheet)); SM.AddState(new FunctionState((int)States.TrainVoice, TrainVoice)); SM.AddState(new FunctionState((int)States.ReceiveSheet, ReceiveSheet)); SM.AddState(new FunctionState((int)States.AdjustTable, AdjustTable)); SM.AddState(new FunctionState((int)States.GotoTray, GotoTray)); SM.AddState(new FunctionState((int)States.TakeTray, TakeTray)); SM.AddState(new FunctionState((int)States.GotoTable, GotoTable)); SM.AddState(new FunctionState((int)States.TrayOnTable, TrayOnTable)); SM.AddState(new FunctionState((int)States.FinalState, FinalState, true)); SM.SetFinalState((int)States.FinalState); }
int TakeObject(int currentState, object o) { string leftArm="",rightArm=""; if (firstObjectTaken) armToUse = (laTaken) ? "right" : "left"; else armToUse = ""; armToUse = "left"; if (objectsToFind[takeCounter].Length >= 7 && objectsToFind[takeCounter].Substring(0, 7).ToLower().Equals("unknown")) SayObjectName = "unknown object"; else SayObjectName = objectsToFind[takeCounter]; //TEMP //brain.SayAsync("I will take the object"); //Thread.Sleep(10000); //reemplazar por bnuevo comando para forzar a que tome un objeto con un brazo dado //if (cmdMan.ST_PLN_takeobject(objectsToFind[takeCounter], armToUse, out string, 180000)) //if (cmdMan.ST_PLN_takeobject(objectsToFind[takeCounter], 180000)) cmdMan.ST_PLN_takeobject(objectsToFind[takeCounter], armToUse, out leftArm, out rightArm, 180000); if (!(leftArm == "empty" && rightArm == "empty")) { brain.SayAsync("I got the " + SayObjectName); TextBoxStreamWriter.DefaultLog.WriteLine("object taken: " + objectsToFind[takeCounter]); foundObjects[takeCounter] = objectsToFind[takeCounter]; //arctualizar la bandera del brazo cupado laTaken} laTaken = (leftArm == "empty") ? false : true; if (!firstObjectTaken) armsOrder[0] = (laTaken) ? "left" : "right"; else armsOrder[1] = (armsOrder[0] == "right") ? "left" : "right"; TextBoxStreamWriter.DefaultLog.WriteLine(objectsToFind[takeCounter] + " using the " + ((!firstObjectTaken) ? armsOrder[0] : armsOrder[1]) + " arm"); } else { if (useTakeHandOver) { TextBoxStreamWriter.DefaultLog.WriteLine("cant take object, using takehandover"); brain.SayAsync(" I cant reach the " + SayObjectName); //brain.SayAsync("I will point at the " + objectsToFind[takeCounter] + "."); //TODO: point at object cmdMan.ST_PLN_takehandover(objectsToFind[takeCounter], 40000); foundObjects[takeCounter] = objectsToFind[takeCounter]; //arctualizar la bandera del brazo cupado laTaken laTaken = (leftArm == "empty") ? false : true; if (!firstObjectTaken) armsOrder[0] = (laTaken) ? "left" : "right"; else armsOrder[1] = (armsOrder[0] == "right") ? "left" : "right"; } else { finalState = FinalStates.FoundButNotTaken; foundObjects[takeCounter] = ""; //TEMP //brain.SayAsync("I cant reach the " + SayObjectName + ", I will look for another object."); //brain.SayAsync("I cant reach the object, I will look for another object."); TextBoxStreamWriter.DefaultLog.WriteLine("Cant take the object, trying to take another one."); objectsFound.RemoveAt(0); return (int)States.SetObjectToTake; } } if (!firstObjectTaken&&twoArms) { TextBoxStreamWriter.DefaultLog.WriteLine("First object Taken."); /*despues de la primer ejecucion checar con que brazo tomó el objeto y actualizar la bandera laTaken(true, toma con el brazo izq, else si no )*/ //laTaken=false takeCounter++; firstObjectTaken = true; objectsFound.RemoveAt(0); return (int)States.SetObjectToTake; } this.finalState = FinalStates.OK; return (int)States.FinalState; }
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 GoToFall(int currentState, object o) { bool notHumanFall; TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> GoToFall state of SM_DetectPersonFall reached."); if(brain.GetCloseToHumanFall(out notHumanFall, 10000)) { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human success."); finalState = FinalStates.OK; return (int)States.FinalState; } else { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human FAILED"); if (!notHumanFall) finalState = FinalStates.FallNotReached; else { //brain.SayAsync("I have detected a fall in that direction."); //point at the direction of the fall //Thread.Sleep(1000); finalState = FinalStates.NoHumanFallDetected; } return (int)States.FinalState; } }
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 EnterArena(int currentState, object o) { this.cmdMan.ARMS_goto("standby", 8000); TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Trying getclose to: " + location); if (this.cmdMan.MVN_PLN_getclose(location, 300000)) { this.finalState = FinalStates.OK; TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Entered the arena"); return (int)States.FinalState; } if (attemptCounter < 3) { attemptCounter++; TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Cannot arrive to entrance location, trying again."); return (int)States.EnterArena; } this.finalState = FinalStates.Failed; TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Cannot arrive to entrance location, SM was NOT successful."); return (int)States.FinalState; }
int TakeObject(int currentState, object o) { if (ObjectToFind.Substring(0, 7).ToLower().Equals("unknown")) SayObjectName = "unknown object"; else SayObjectName = ObjectToFind; if(this.succesGetClose) { brain.SayAsync("I will take the " + SayObjectName); Thread.Sleep(5000); if (cmdMan.ST_PLN_takeobject(ObjectToFind, 105000)) { TextBoxStreamWriter.DefaultLog.WriteLine("object taken"); } else { TextBoxStreamWriter.DefaultLog.WriteLine("cant take object, using takehandover"); brain.SayAsync(" I cant take the " + SayObjectName); cmdMan.ST_PLN_takehandover(ObjectToFind, 40000); } } else { Thread.Sleep(5000); TextBoxStreamWriter.DefaultLog.WriteLine("Getclose not succesfull, using takehandover"); brain.SayAsync("I can't take the " + SayObjectName); cmdMan.ST_PLN_takehandover(ObjectToFind, 40000); } this.finalState = FinalStates.OK; return (int)States.FinalState; }
private int GoToRequest(int currentState, object o) { if (attemptCounter < 3) { if (cmdMan.MVN_PLN_getclose("human " + requestId, 60000)) { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human success at attempt" + attemptCounter); finalState = FinalStates.OK; return (int)States.FinalState; } else { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human FAILED at attempt" + attemptCounter); attemptCounter++; return currentState; } } attemptCounter = 0; finalState = FinalStates.HumanNotReachable; return (int)States.FinalState; }
int SetObjectToTake(int currentState, object o) { objectFoundIndex = 0; this.foundObjectsCount = (byte)objectsFound.Count; if (this.foundObjectsCount == 0) { if (!firstObjectTaken) { TextBoxStreamWriter.DefaultLog.WriteLine("No reachable objects were found, SM was not successful."); this.finalState = FinalStates.Failed; return (int)States.FinalState; } else { this.finalState = FinalStates.OK; return (int)States.FinalState; } } if (!anyObject) { if (objectsFound.Contains(objectsToFind[takeCounter])) { objectFoundIndex = objectsFound.IndexOf(objectsToFind[takeCounter]); TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object found."); return (int)States.TakeObject; } ///******28-05-2013*******//////////// if (searchAttemptCounter < searchAttempts ) { searchAttemptCounter++; TextBoxStreamWriter.DefaultLog.WriteLine("object NOT found, trying again"); this.brain.SayAsync("I did not find the " + objectsToFind[takeCounter] + ". I will try again"); //this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000); Thread.Sleep(2500); return (int)States.SearchCloseObjects; } TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, test failed."); this.finalState = FinalStates.Failed; return (int)States.FinalState; } objectsToFind[takeCounter] = objectsFound[0]; attemptCounter = 0; TextBoxStreamWriter.DefaultLog.WriteLine("Some Object found."); return (int)States.TakeObject; }
private int PersiguiendoPersona(int currentState, object o) { double distanc =Math.Sqrt(Math.Pow(hum.X, 2) + Math.Pow(hum.Y, 2)); if (distanc >= 0.5) { distanc = distanc-0.10; } else if (distanc >= 0.3 && distanc < 0.5) { distanc = 0; } else if (distanc<0.3) { distanc = -1; } double ang = 0; if (hum.X == 0) { if (hum.Y > 0) { ang = Math.PI / 2; } else { ang = -Math.PI / 2; } } else { ang = Math.Atan2(hum.Y, hum.X); if (Math.Abs(ang) < AnguloEstable) { ang = 0; } } if (distanc < 0) { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> por fin"); finalState = FinalStates.HumanReached; return (int)States.AntesDeTerminar; } else if(distanc==0) { return (int)States.SensandoPersona; } else { this.cmdMan.MVN_PLN_move(distanc, ang); TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> siguiendo a la persona"); return (int)States.SensandoPersona; } }
int SearchCloseObjects(int currentState, object o) { List<string> objectsFound = new List<string>(); if (cmdMan.ST_PLN_fashionfind_object(ObjectToFind, 90000, out objectsFound)) { TextBoxStreamWriter.DefaultLog.WriteLine("Objects found."); this.foundObjectsCount = (byte)objectsFound.Count; if (!anyObject) { if (objectsFound.Contains(ObjectToFind)) { TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object found."); attemptCounter = 0; return (int)States.TakeObject; } if (attemptCounter < 3) { attemptCounter++; TextBoxStreamWriter.DefaultLog.WriteLine("object NOT found, trying again"); this.brain.SayAsync("I did not find the " + ObjectToFind + ". I will try again"); //this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000); Thread.Sleep(2500); return currentState; } TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, will try to continue the test."); attemptCounter = 0; this.finalState = FinalStates.Failed; return (int)States.FinalState; /*foreach (string obj in objectsFound) { //cmdMan.SPG_GEN_say(obj); if (obj == objectToBring) { TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object founded"); nextStage = 70; founded = true; } }*/ } else { ObjectToFind = objectsFound[0]; foundObject = objectsFound[0]; attemptCounter = 0; TextBoxStreamWriter.DefaultLog.WriteLine("Some Object found."); return (int)States.TakeObject; } } else { if (attemptCounter < 3) { attemptCounter++; TextBoxStreamWriter.DefaultLog.WriteLine("No objects were found, trying again"); this.brain.SayAsync("I did not find the " + ObjectToFind + ". I will try again"); this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000); Thread.Sleep(2500); return currentState; } this.brain.SayAsync("I did not find " + (anyObject?"any":"") + ObjectToFind + " in " + objectLocation); TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, SM was not successful."); attemptCounter = 0; this.finalState = FinalStates.Failed; return (int)States.FinalState; } }
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 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; }
private int AsociateName(int currentState, object o) { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Confirm \"yes\" received"); brain.SayAsync("O.K. I am going to remember your face. Please look straight forward to my eyes"); TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Sending <remember_human> command"); if (this.cmdMan.ST_PLN_rememberhuman(foundHuman, "", 60000)) { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Face of " + foundHuman + " remembered succesfully"); this.cmdMan.SPG_GEN_say("I have remembered your face.", 10000); this.cmdMan.HEAD_lookat(0, 0, 4000); this.cmdMan.MVN_PLN_position(out x, out y, out angle, 2000); } else { TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Cannot remember the face of " + foundHuman); this.cmdMan.SPG_GEN_say("I cannot remember your face. I will continue with the test", 12000); this.cmdMan.HEAD_lookat(0, 0, 4000); } TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumandRoutine.-> Desactivating reco human"); this.cmdMan.PRS_FND_sleep(true); this.finalState = FinalStates.OK; return (int)States.FinalState; }
int TakeObject(int currentState, object o) { if (ObjectToFind.Length >= 7 && ObjectToFind.Substring(0, 7).ToLower().Equals("unknown")) SayObjectName = "unknown object"; else SayObjectName = ObjectToFind; brain.SayAsync("I will take the " + SayObjectName); Thread.Sleep(5000); if (cmdMan.ST_PLN_takeobject(ObjectToFind, 180000)) { TextBoxStreamWriter.DefaultLog.WriteLine("object taken"); } else { if (useTakeHandOver) { TextBoxStreamWriter.DefaultLog.WriteLine("cant take object, using takehandover"); brain.SayAsync(" I cant reach the " + SayObjectName); cmdMan.ST_PLN_takehandover(ObjectToFind, 40000); } else { brain.SayAsync("I cant reach the " + SayObjectName + ", I will look for another object."); objectsFound.RemoveAt(objectFoundIndex); TextBoxStreamWriter.DefaultLog.WriteLine("Cant take the object, trying to take another one."); return (int)States.SetObjectToTake; } } this.finalState = FinalStates.OK; return (int)States.FinalState; }
int SearchCloseObjects(int currentState, object o) { if (cmdMan.ST_PLN_fashionfind_object(objectsToFind[takeCounter], 270000, out objectsFound)) { attemptCounter = 0; TextBoxStreamWriter.DefaultLog.WriteLine("Objects found."); this.totalFoundObjects = (byte)objectsFound.Count; return (int)States.SetObjectToTake; } /***28-05-2013*//// if (attemptCounter < searchAttempts) { attemptCounter++; TextBoxStreamWriter.DefaultLog.WriteLine("No objects were found, trying again"); this.brain.SayAsync("I did not find the " + objectsToFind[takeCounter] + ". I will try again"); //this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000); Thread.Sleep(2500); return currentState; } this.brain.SayAsync("I did not find " + (anyObject ? "any " : "the ") + objectsToFind[takeCounter]); TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, SM was not successful."); attemptCounter = 0; this.finalState = FinalStates.Failed; return (int)States.FinalState; }
private int LeaveArena(int currentState, object o) { this.cmdMan.ARMS_goto("standby", 8000); if (cmdMan.MVN_PLN_getclose(exitLocation, 120000)) { TextBoxStreamWriter.DefaultLog.WriteLine("Location reached: " + exitLocation); this.finalState = FinalStates.OK; return (int)States.FinalState; } if (attemptCounter < 3) { attemptCounter++; TextBoxStreamWriter.DefaultLog.WriteLine("Cant reach location: " + exitLocation + ", trying again"); return currentState; } TextBoxStreamWriter.DefaultLog.WriteLine("Cant reach location: " + exitLocation + ", SM execution was NOT successful"); this.finalState = FinalStates.Failed; return (int)States.FinalState; }
int SearchCloseObjects(int currentState, object o) { if (cmdMan.ST_PLN_fashionfind_object(ObjectToFind, 180000, out objectsFound)) { attemptCounter = 0; TextBoxStreamWriter.DefaultLog.WriteLine("Objects found."); return (int)States.SetObjectToTake; } if (attemptCounter < 2) { attemptCounter++; TextBoxStreamWriter.DefaultLog.WriteLine("No objects were found, trying again"); this.brain.SayAsync("I did not find the " + ObjectToFind + ". I will try again"); //this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000); Thread.Sleep(2500); return currentState; } this.brain.SayAsync("I did not find " + (anyObject?"any":"") + ObjectToFind); TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, SM was not successful."); attemptCounter = 0; this.finalState = FinalStates.Failed; return (int)States.FinalState; }