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