/// <summary>
/// 移动
/// </summary>
/// <param name="Elc"></param>
/// <returns></returns>
public MoveType Move(ElecMap Elc)
{
lock (RouteLock)
{
if (route == null || route.Count < 1)
{
return(MoveType.arrived);
}
if (TPtr >= route.Count - 1)
{
Elc.mapnode[route[route.Count - 1].X, route[route.Count - 1].Y].NodeCanUsed = this.Id;
Arrive = true;
return(MoveType.arrived);
//Elc.mapnode[route[route.Count - 1].X, route[route.Count - 1].Y].NodeCanUsed = this.Id;
//if (EqualWithRealLocation(route[route.Count - 1].X, route[route.Count - 1].Y))
//{
// Arrive = true;
// return MoveType.arrived;
//}
//else
//{
// return MoveType.move;
//}
}
#if moni
#else
if (!CheckAgvCorrect())
{
return(MoveType.agvFault);
}
////TODO://如果当前方向就是小车方向给它发转弯是否会出错？转向完成，翻盘完成的标志是否要用？
//if (!swerverFinished)
//{
// return MoveType.cannotReceiveRunCommands;
//}
//dir = GetAgvDireciton();
//Console.WriteLine(dir.ToString());
//if (TPtr == 0 && route[0].Dir != dir)
//{
// if (AgvCanReceiveSwerverCommands())
// {
// return MoveType.move;
// swerverFinished = false;
// return GetSwerveAngle(route[0].Dir);
// }
// else
// {
// return MoveType.cannotReceiveSwerverCommands;
// }
//}
//if (route[TPtr + 1].Dir != dir)
//{
// if (AgvCanReceiveSwerverCommands())
// {
// return MoveType.move;
// swerverFinished = false;
// return GetSwerveAngle(route[TPtr + 1].Dir);
// }
// else
// {
// return MoveType.cannotReceiveSwerverCommands;
// }
//}
if (!AgvCanReceiveRunCommands())
{
return(MoveType.cannotReceiveRunCommands);
}
if (ShouldMove(TPtr + 1) == false)
{
BeginX = route[TPtr].X;
BeginY = route[TPtr].Y;
if (this.WaitEndTime < DateTime.Now)//超过等待时间还不能走，则重新发送一下当前位置
{
Console.WriteLine("Resend Current location");
return(MoveType.move);
}
return(MoveType.cannotReceiveRunCommands);
}
#endif
MyPoint cur = new MyPoint(RealX, RealY, dir);
int index = 0;
for (; index < route.Count; index++)
{
if (cur.Equals(route[index]))
{
break;
}
}
if (index >= route.Count - 1)
{
index = route.Count - config.ForwordStep;
}
bool canMove = false;
Direction virtualDir = route[index + 1].Dir;
for (VirtualTPtr = index + 1; VirtualTPtr < index + config.ForwordStep; VirtualTPtr++)
{
if (VirtualTPtr <= route.Count - 1)
{
int tx = (int)route[VirtualTPtr].X;
int ty = (int)route[VirtualTPtr].Y;
Direction tDir = route[VirtualTPtr].Dir;
Boolean IsCanMoveTo = Elc.IsVehicleCanMove(this, tx, ty);// Elc.mapnode[tx, ty].NodeCanUsed;
if (IsCanMoveTo && virtualDir == tDir)
{
canMove = true;
virtualDir = tDir;
Elc.mapnode[tx, ty].NodeCanUsed = this.Id;
}
else
{
break;
}
}
}
if (canMove)
{
#if moni
//if (TPtr == 0)
//{
// ElecMap.Instance.mapnode[BeginX, BeginY].NodeCanUsed = -1;
//}
ElecMap.Instance.mapnode[BeginX, BeginY].NodeCanUsed = -1;
TPtr++;
BeginX = route[TPtr].X;
BeginY = route[TPtr].Y;
#else
TPtr++;
BeginX = route[TPtr].X;
BeginY = route[TPtr].Y;
#endif
return(MoveType.move);
}
else
{
StopTime--;
return(MoveType.stopAvoidConflict);
}
#region old
/*
* if (TPtr == 0)// config.ForwordStep)
* {
*
* for (VirtualTPtr = 1; VirtualTPtr < config.ForwordStep; VirtualTPtr++)
* {
* if (TPtr + VirtualTPtr <= route.Count - 1)
* {
* int tx = (int)route[VirtualTPtr].X;
* int ty = (int)route[VirtualTPtr].Y;
* int temp = Elc.mapnode[tx, ty].NodeCanUsed;
* if (temp > -1)
* {
* Stoped = temp;
* StopTime--;
* return false;
* }
* else
* {
* Elc.mapnode[tx, ty].NodeCanUsed = this.Id;
* }
* }
* }
* StopTime = StopTime;
* TPtr++;
*
* }
* else if (TPtr > 0)
* {
*
* if (VirtualTPtr <= route.Count - 1)
* {
* int tx = (int)route[VirtualTPtr].X;
* int ty = (int)route[VirtualTPtr].Y;
* int temp = Elc.mapnode[tx, ty].NodeCanUsed;
* if (temp > -1)
* {
* Stoped = temp;
* StopTime--;
* return false;
* }
* else
* {
* Elc.mapnode[tx, ty].NodeCanUsed = this.Id;
* StopTime = StopTime;
* TPtr++;
* VirtualTPtr++;
* }
*
* }
* else
* {
* StopTime = StopTime;
* TPtr++;
* }
* }
* BeginX = route[TPtr].X;
* BeginY = route[TPtr].Y;
* return true;
*/
#endregion
}
}
/// <summary>
/// 移动
/// </summary>
/// <param name="Elc"></param>
/// <returns></returns>
public MoveType Move(ElecMap Elc)
{
lock (RouteLock)
{
if (route == null || route.Count < 1)
{
return(MoveType.arrived);
}
if (TPtr >= route.Count - 1)
{
SetNodeOccpyed(route[route.Count - 1].X, route[route.Count - 1].Y);
Arrive = true;
return(MoveType.arrived);
if (EqualWithRealLocation(route[route.Count - 1].X, route[route.Count - 1].Y))
{
Arrive = true;
return(MoveType.arrived);
}
else
{
if (agvInfo.OrderExec == OrderExecState.Free)
{
return(MoveType.move);
}
else
{
return(MoveType.arrived);
}
}
}
#if moni
#else
MoveType checkResult = CheckAgvCorrect();
if (checkResult != MoveType.move)
{
if (checkResult == MoveType.clearFault)
{
if (clearFaultCount < 0)
{
return(MoveType.agvFault);
}
else
{
clearFaultCount--;
return(MoveType.clearFault);
}
}
return(checkResult);
}
clearFaultCount = MAX_CLEAR_FAULT_COUNT;
////TODO://如果当前方向就是小车方向给它发转弯是否会出错？转向完成，翻盘完成的标志是否要用？
//if (!swerverFinished)
//{
// return MoveType.cannotReceiveRunCommands;
//}
//dir = GetAgvDireciton();
//Console.WriteLine(dir.ToString());
//if (TPtr == 0 && route[0].Dir != dir)
//{
// if (AgvCanReceiveSwerverCommands())
// {
// return MoveType.move;
// swerverFinished = false;
// return GetSwerveAngle(route[0].Dir);
// }
// else
// {
// return MoveType.cannotReceiveSwerverCommands;
// }
//}
//if (route[TPtr + 1].Dir != dir)
//{
// if (AgvCanReceiveSwerverCommands())
// {
// return MoveType.move;
// swerverFinished = false;
// return GetSwerveAngle(route[TPtr + 1].Dir);
// }
// else
// {
// return MoveType.cannotReceiveSwerverCommands;
// }
//}
if (!AgvCanReceiveRunCommands())
{
return(MoveType.cannotReceiveRunCommands);
}
if (ShouldMove(TPtr + 1) == false)
{
return(MoveType.cannotReceiveSwerverCommands);
}
#endif
bool canMove = false;
Direction virtualDir = route[TPtr + 1].Dir;
for (VirtualTPtr = TPtr + 1; VirtualTPtr < TPtr + config.ForwordStep; VirtualTPtr++)
{
if (VirtualTPtr <= route.Count - 1)
{
if (!VitrualStepCanMove(VirtualTPtr))
{
break;
}
int tx = (int)route[VirtualTPtr].X;
int ty = (int)route[VirtualTPtr].Y;
Direction tDir = route[VirtualTPtr].Dir;
Boolean IsCanMoveTo = Elc.IsVehicleCanMove(this, tx, ty);// Elc.mapnode[tx, ty].NodeCanUsed;
if (IsCanMoveTo && virtualDir == tDir)
{
canMove = true;
virtualDir = tDir;
bool res = SetNodeOccpyed(tx, ty);
if (res == false)
{
break;
}
if (agvInfo.OrderExec != OrderExecState.Run)
{
break;
}
}
else
{
break;
}
}
}
if (canMove)
{
#if moni
//if (TPtr == 0)
//{
// ElecMap.Instance.mapnode[BeginX, BeginY].NodeCanUsed = -1;
//}
ElecMap.Instance.mapnode[BeginX, BeginY].NodeCanUsed = -1;
TPtr++;
BeginX = route[TPtr].X;
BeginY = route[TPtr].Y;
#else
StopTime = ConstDefine.STOP_TIME;
TPtr++;
BeginX = route[TPtr].X;
BeginY = route[TPtr].Y;
return(MoveType.move);
#endif
}
else
{
StopTime--;
return(MoveType.stopAvoidConflict);
}
}
}
