public void Tick()
{
if (robot.IsMoving)
{
moved = true;
}
else
{
if (moved)
{
moved = false;
if (rallyPoints.Count > 0)
{
var robotl = rallyPoints[0];
robot.ReadLaserValues();
Console.WriteLine("Robot: {0} {1}", robot.Center.x, robot.Center.x);
Console.WriteLine("Robot: {0} {1} {2} {3}", robot.LL, robot.LF, robot.RF, robot.RR);
// Render();
Point loc = locationApproximator.GetLocation((int)(robotl.x - R / 2), (int)(robotl.y - R / 2), R, R, 2, robot.LL, robot.LF, robot.RR, robot.RF);
if (loc != null)
{
pathFinder.setSrc((int)loc.x, (int)loc.y);
pathFinder.findPath();
robot.Center = loc;
Console.WriteLine("rec");
}
// path = new PathShape();
// Console.WriteLine("Path Hash" + path.GetHashCode());
// path.path = pathFinder.getPath();
// route.path = path;
rallyPoints = NormalizePath(pathFinder.getPath());
rallyPoints.RemoveAt(0);
while (rallyPoints.Count > 1)
{
double dx = rallyPoints[0].x - robot.Center.x;
double dy = rallyPoints[0].y - robot.Center.y;
if (dx * dx + dy * dy < 4)
{
rallyPoints.RemoveAt(0);
}
else
{
break;
}
}
if (rallyPoints.Count > 0)
{
double dx = rallyPoints[0].x - robot.Center.x;
double dy = rallyPoints[0].y - robot.Center.y;
Console.WriteLine((float)(dx));
Console.WriteLine((float)(dy));
if (Math.Abs(dx) <= Math.Abs(dy))
{
dx = 0;
}
else
{
dy = 0;
}
if (Math.Sqrt(dx * dx + dy * dy) < 15)
{
robot.Speed = 5;
}
else
{
robot.Speed = 10;
}
double fx = pathFinder.getDst().x - robot.Center.x;
double fy = pathFinder.getDst().y - robot.Center.y;
robot.Go((float)(dx), (float)(-dy));
}
}
}
}
}