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