private void GoTo(PointF point)
{
var angle = NormalRelativeAngleRadians(
AbsoluteBearingRadians(GetRobotLocation(), point) - HeadingRadians);
SetTurnRightRadians(angle);
Execute();
if (Math.Abs(angle) > 0.003)
{
Wait();
}
SetAhead(GetRobotLocation().Distance(point));
Execute();
if (GetRobotLocation().Distance(point) < 0.3)
{
_atNode = true;
_lenght++;
}
}
private static double AbsoluteBearingRadians(PointF source, PointF target)
{
return(Math.Atan2(target.X - source.X, target.Y - source.Y));
}
