bool ShouldMove(int nextTPtr)
{
if (agvInfo == null)
{
return(false);
}
if (agvInfo.Alarm != AlarmState.Normal)
{
return(false);
}
if (nextTPtr >= route.Count)
{
return(false);
}
if (nextTPtr == 0)
{
return(true);
}
SetCurAndNextDirection(nextTPtr);
if (nextTPtr > 1)
{
if (curMoveDirection != nextMoveDirection)
{
if (agvInfo.AgvMotion == AgvMotionState.StopedNode)
{
Console.WriteLine("stoped!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1");
SwerveStoped = true;
curMoveDirection = nextMoveDirection;
}
else
{
Console.WriteLine("还没停止...");
SwerveStoped = false;
return(false);
}
}
}
int nextX = route[nextTPtr].X;
int nextY = route[nextTPtr].Y;
double RealX = agvInfo.CurLocation.CurNode.X / 1000.0;
double RealY = agvInfo.CurLocation.CurNode.Y / 1000.0;
if (Math.Abs(nextX - RealX) < ConstDefine.DEVIATION + ConstDefine.FORWORD_STEP - 1 && Math.Abs(nextY - RealY) < ConstDefine.DEVIATION)//X轴移动
{
return(true);
}
if (Math.Abs(nextX - RealX) < ConstDefine.DEVIATION && Math.Abs(nextY - RealY
) < ConstDefine.DEVIATION + ConstDefine.FORWORD_STEP - 1)//Y轴移动
{
return(true);
}
return(false);
}
//public void SetCurDirectionEqualNext(byte serinum)
// {
// curMoveDirection = nextMoveDirection;
// }
void SetCurAndNextDirection(int index)
{
if (SwerveStoped == false)
{
return;
}
if (Math.Abs(route[index].X - route[index - 1].X) == 0 && Math.Abs(route[index].Y - route[index - 1].Y) == 1)//Y轴方向
{
curMoveDirection = nextMoveDirection;
nextMoveDirection = MoveDirecion.YDirection;
}
if (Math.Abs(route[index].X - route[index - 1].X) == 1 && Math.Abs(route[index].Y - route[index - 1].Y) == 0)//X轴方向
{
curMoveDirection = nextMoveDirection;
nextMoveDirection = MoveDirecion.XDirection;
}
}