public void Tick()
{
if (orientations.Count == 0)
{
log.Info("BNT FINISHED");
return;
}
var d = robot.Center - route.GetDestination();
var orientation = orientations.First();
switch (state)
{
case BNTState.ROTATING:
if (robot.IsMoving)
{
return;
}
if (robot.Orientation != orientation)
{
robot.Rotate(88);
log.Info(robot.Orientation);
}
else
{
state = BNTState.WAITING;
startTime = DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond;
}
break;
case BNTState.MOVING:
if (d.Lenght() < 6)
{
state = BNTState.ROTATING;
break;
}
route.Tick();
break;
case BNTState.CORRECTING:
if (robot.IsMoving)
{
return;
}
if (robot.Orientation != Orientation.N)
{
robot.Rotate(88);
log.Info(robot.Orientation);
}
else
{
state = BNTState.MOVING;
orientations.RemoveAt(0);
names.RemoveAt(0);
if (names.Count > 0)
{
StartRoutingTo(robot.Center, names[0]);
}
}
break;
case BNTState.WAITING:
var now = DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond;
if (now - startTime > 5000)
{
state = BNTState.CORRECTING;
}
break;
}
if (state == BNTState.MOVING)
{
route.Tick();
}
}