/// <summary>
/// Change direction towards the marker and return true iff we reached our destination.
/// </summary>
/// <param name="marker"></param>
/// <returns></returns>
private bool Calculate(ArrayList marker)
{
bool reached = false;
float angle = ((float)((ArrayList)marker[0])[1]) / 3F;
Call(() => Walk.Instance.StartWalking(speed, 0, Math.Max(-1, Math.Min(1, angle))));
float sizeY = ((float)((ArrayList)marker[0])[4]);
if (MarkerRecogniser.estimateDistance(sizeY) <= (double)dist)
{
Logger.Log("At Correct Distance: " + dist);
reached = true;
}
return(reached);
}
public void LookForMarker()
{
Pose.Instance.Look(0f);
MarkerRecogniser rec = MarkerRecogniser.Instance;
Sonar sonar = Sonar.Instance;
ArrayList markers;
Logger.Log(this, "Look for marker");
Call(() => Walk.Instance.StartWalking(speed, 0, 0));
while (Running && looking)
{
Thread.Sleep(1000);// update time of nao LandMarkDetection
ArrayList data = rec.GetMarkerData();
markers = data.Count == 0 ? data : (ArrayList)data[1];
CheckMarkers(markers);
// dont check sensor when holding object, it will probably block the sonar.
if (!NaoState.Instance.HoldingObject && markers.Count == 0)
{
CheckSonar(sonar);
}
}
Walk.Instance.StopMoving();
Logger.Log("Exit LookForMarker : " + Running);
}