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