void HandleData(object sender, MessageEventArgs e)
{
string[] location = e.Message.Split(':');
uint x = Convert.ToUInt32(location[0]);
uint y = Convert.ToUInt32(location[1]);
uint endX = Convert.ToUInt32(location[2]);
uint endY = Convert.ToUInt32(location[3]);
if (e.Type == MessageType.move)
{
// TrayPacket tp = new TrayPacket(1, 4, TrayMotion.TopLeft);
RunPacket rp = new RunPacket(1, 4, MoveDirection.Forward, 1500, new Destination(new MyPoint(x * ConstDefine.CELL_UNIT, y * ConstDefine.CELL_UNIT), new MyPoint(endX * ConstDefine.CELL_UNIT, endY * ConstDefine.CELL_UNIT), new AgvDriftAngle(90), TrayMotion.TopLeft));
//asm.Send(rp);
SendPacketQueue.Instance.Enqueue(rp);
Console.WriteLine(x + "," + y + "->" + endX + "," + endY);
}
else if (e.Type == MessageType.reStart)
{
TrayPacket tp = new TrayPacket(1, 4, TrayMotion.TopLeft);
// asm.Send(tp);
SendPacketQueue.Instance.Enqueue(tp);
}
//else if (e.Type == MessageType.none)//stop
//{
// RunPacket rp = new RunPacket(1, 4, MoveDirection.Forward, 0, new Destination(new MyPoint(x * ConstDefine.CELL_UNIT, y * ConstDefine.CELL_UNIT), new MyPoint(x * ConstDefine.CELL_UNIT, y * ConstDefine.CELL_UNIT), new AgvDriftAngle(90), TrayMotion.None));
// asm.Send(rp);
//}
}
protected override void Run()
{
Thread.Sleep(ConstDefine.STEP_TIME);
if (vehicles == null)
{
return;
}
for (int vnum = 2; vnum <= 16; vnum++)
{
if (vehicles[vnum].agvInfo == null)
{
continue;
}
// Console.WriteLine(vnum + "小车电量:" + vehicles[vnum].agvInfo.Electricity);
if (vehicles[vnum].Route == null || vehicles[vnum].Route.Count <= 0)
{
continue;
}
if (vehicles[vnum].CurState == State.cannotToDestination && vehicles[vnum].Arrive == false)
{
vehicles[vnum].Arrive = true;
vehicles[vnum].CurState = State.Free;
vFinished.Add(vehicles[vnum]);
ClearAllCrossedNode(vnum);
string str = string.Format("小车" + vnum + ":{0},{1}->{2},{3}没有搜索到路径，", vehicles[vnum].BeginX, vehicles[vnum].BeginY, vehicles[vnum].EndX, vehicles[vnum].EndY);
OnShowMessage(this, new MessageEventArgs(str));
continue;
}
if (vehicles[vnum].Finished == true)
{
continue;
}
if (vehicles[vnum].IsAgvReceived == false)
{
if (vehicles[vnum].WaitEndTime < DateTime.Now)//超过等待时间还不能走，则重新发送一下当前位置
{
Logs.Warn("Resend Current location");
// MessageBox.Show("resend " + vnum);
SendPack(vnum, vehicles[vnum].LastSendPacket);
}
continue;
}
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.NeedCharge)
{
if (vehicles[vnum].agvInfo.OrderExec == OrderExecState.Free &&
vehicles[vnum].EqualWithRealLocation(CHARGE_END_X, CHARGE_END_Y)
)
{
uint cellUnit = (int)ConstDefine.CELL_UNIT;
ChargePacket cp = new ChargePacket((byte)(vehicles[vnum].Serinum), (byte)vnum, new CellPoint(CHARGING_X * cellUnit, CHARGING_Y * cellUnit));
SendPack(vnum, cp);
vehicles[vnum].CurState = State.Charging;
}
continue;
}
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.Charging)
{
if (vehicles[vnum].agvInfo.Electricity > chargeMax)
{
if (vehicles[vnum].agvInfo.OrderExec == OrderExecState.Free &&
vehicles[vnum].EqualWithRealLocation(CHARGING_X, CHARGING_Y)
)
{
uint cellUnit = (int)ConstDefine.CELL_UNIT;
FinishChargePacket fcp = new FinishChargePacket((byte)(vehicles[vnum].Serinum), (byte)vnum, new CellPoint(CHARGE_END_X * cellUnit, CHARGE_END_Y * cellUnit));
SendPack(vnum, fcp);
vehicles[vnum].CurState = State.Charged;
}
}
continue;
}
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.Charged)
{
if (vehicles[vnum].EqualWithRealLocation(CHARGE_END_X, CHARGE_END_Y) && vehicles[vnum].agvInfo.OrderExec == OrderExecState.Free)
{
vehicles[vnum].CurState = State.Free;
}
continue;
}
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.carried)
{
if (vehicles[vnum].EqualWithRealLocation(vehicles[vnum].BeginX, vehicles[vnum].BeginY))
{
if (vehicles[vnum].agvInfo.OrderExec == OrderExecState.Free)
{
TrayMotion trayMotion = TrayMotion.YNegative;
TrayPacket tp = new TrayPacket((byte)(vehicles[vnum].Serinum), (ushort)vnum, trayMotion);
SendPack(vnum, tp);
vehicles[vnum].CurState = State.unloading;
}
}
continue;
}
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.unloading)
{
if (vehicles[vnum].agvInfo.OrderExec == OrderExecState.Free &&
(vehicles[vnum].LastSendPacket is TrayPacket || vehicles[vnum].LastSendPacket is FinishChargePacket)
)
{
if (vehicles[vnum].agvInfo.Electricity < chargeMin)
{
GoToCharge(vnum);
}
else
{
vehicles[vnum].CurState = State.Free;
}
}
continue;
}
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.Free)
{
ClearAllCrossedNode(vnum);
vehicles[vnum].Finished = true;
vFinished.Add(vehicles[vnum]);
continue;
}
Random r = new Random(DateTime.Now.Millisecond);
if (vehicles[vnum].StopTime < r.Next(vnum % 5))
{
if (vehicles[vnum].CurNodeTypy() != MapNodeType.queuingArea && GetDirCount(vehicles[vnum].BeginX, vehicles[vnum].BeginY) > 1)
{
if (vehicles[vnum].Stoped > -1 && vehicles[vnum].Stoped < vehicles.Length)
{
vehicles[vehicles[vnum].Stoped].StopTime = ConstDefine.STOP_TIME;
}
//重新搜索路径
SearchRoute(vnum, true);
}
vehicles[vnum].StopTime = ConstDefine.STOP_TIME;
}
else
{
MoveType moveState = vehicles[vnum].Move(ElecMap.Instance);
if (moveState == MoveType.move)
{
uint x = Convert.ToUInt32(vehicles[vnum].BeginX);
uint y = Convert.ToUInt32(vehicles[vnum].BeginY);
uint endX = Convert.ToUInt32(vehicles[vnum].EndX);
uint endY = Convert.ToUInt32(vehicles[vnum].EndY);
uint cellUnit = (int)ConstDefine.CELL_UNIT;
RunPacket rp = new RunPacket((byte)(vehicles[vnum].Serinum), (ushort)vnum, MoveDirection.Forward, 1500, new Destination(new CellPoint(x * cellUnit, y * cellUnit), new CellPoint(endX * cellUnit, endY * cellUnit), new AgvDriftAngle(0), TrayMotion.None));
SendPack(vnum, rp);
ElecMap.Instance.mapnode[vehicles[vnum].BeginX, vehicles[vnum].BeginY].TraCongesIntensity = 100;//实际场地没用，故用来表示已经发送过的节点的标志，便于在界面上绘制
moveCount++;
OnShowMessage(string.Format("{0:N} 公里", (moveCount * 1.5) / 1000.0));
}
if (moveState == MoveType.clearFault)
{
if (vehicles[vnum].agvInfo.OrderExec == OrderExecState.Free
)
{
ClearFalutPacket cf = new ClearFalutPacket(vehicles[vnum].Serinum, (ushort)vnum);
SendPack(vnum, cf);
}
}
}
}
if (vFinished != null)
{
for (int i = 0; i < vFinished.Count; i++)
{
FinishedQueue.Instance.Enqueue(vFinished[i]);
}
vFinished.Clear();
}
}
protected override void Run()
{
#if moni
Thread.Sleep(100);
#else
Thread.Sleep(ConstDefine.STEP_TIME);
#endif
if (vehicles == null)
{
return;
}
for (int vnum = 4; vnum < vehicles.Length - 1; vnum++)
{
serinum = (byte)(serinum % 255);
if (vehicles[vnum].CurState == State.cannotToDestination && vehicles[vnum].Arrive == false)
{
vehicles[vnum].Arrive = true;
vFinished.Add(vehicles[vnum]);
string str = string.Format("小车" + vnum + ":{0},{1}->{2},{3}没有搜索到路径，", vehicles[vnum].BeginX, vehicles[vnum].BeginY, vehicles[vnum].EndX, vehicles[vnum].EndY);
OnShowMessage(this, new MessageEventArgs(str));
continue;
}
if (vehicles[vnum].Finished == true)
{
continue;
}
#if moni
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.carried)
{
//vehicle[vnum].BeginX = vehicle[vnum].EndX;
//vehicle[vnum].BeginY = vehicle[vnum].EndY;
vehicles[vnum].CurState = State.unloading;
vFinished.Add(vehicles[vnum]);
vehicles[vnum].Route.Clear();
vehicles[vnum].LockNode.Clear();
continue;
}
if (vehicles[vnum].Arrive == true)
{
//vehicle[vnum].BeginX = vehicle[vnum].EndX;
//vehicle[vnum].BeginY = vehicle[vnum].EndY;
//vehicle[vnum].vehical_state = State.unloading;
vFinished.Add(vehicles[vnum]);
vehicles[vnum].Route.Clear();
vehicles[vnum].LockNode.Clear();
continue;
}
#else
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.carried)
{
if (vehicles[vnum].EqualWithRealLocation(vehicles[vnum].BeginX, vehicles[vnum].BeginY))
{
if (vehicles[vnum].agvInfo.AgvMotion == AgvMotionState.StopedNode)
{
TrayPacket tp = new TrayPacket((byte)(serinum * vnum), (ushort)vnum, TrayMotion.TopLeft);
AgvServerManager.Instance.SendTo(tp, vnum);
vehicles[vnum].CurState = State.unloading;
vehicles[vnum].WaitEndTime = DateTime.Now.AddSeconds(WAIT_TIME * 2);
Console.WriteLine(vnum + ":send TrayMotion:" + (serinum - 1));
}
continue;
}
}
//if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.unloading)
//{
// if (vehicles[vnum].WaitEndTime < DateTime.Now)//超过等待时间还不能走，则重新发送一下倒货
// {
// if (vehicles[vnum].EqualWithRealLocation(vehicles[vnum].BeginX, vehicles[vnum].BeginY))
// {
// if (vehicles[vnum].agvInfo.AgvMotion == AgvMotionState.StopedNode)
// {
// TrayPacket tp = new TrayPacket((byte)(serinum * vnum), (ushort)vnum, TrayMotion.TopLeft);
// AgvServerManager.Instance.SendTo(tp, vnum);
// vehicles[vnum].CurState = State.unloading;
// vehicles[vnum].WaitEndTime = DateTime.Now.AddSeconds(WAIT_TIME);
// Console.WriteLine(vnum + ":resend TrayMotion:" + (serinum - 1));
// }
// continue;
// }
// }
//}
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.Free)
{
vFinished.Add(vehicles[vnum]);
vehicles[vnum].LockNode.Clear();
vehicles[vnum].Finished = true;
continue;
}
#endif
if (vehicles[vnum].StopTime < 0)
{
if (vehicles[vnum].CurNodeTypy() != MapNodeType.queuingArea && GetDirCount(vehicles[vnum].BeginX, vehicles[vnum].BeginY) > 1)
{
if (vehicles[vnum].Stoped > -1 && vehicles[vnum].Stoped < vehicles.Length)
{
vehicles[vehicles[vnum].Stoped].StopTime = 2;
}
//重新搜索路径
SearchRoute(vnum, true);
}
vehicles[vnum].StopTime = 3;
}
else
{
#if moni
vehicles[vnum].Move(ElecMap.Instance);
#else
MoveType moveState = vehicles[vnum].Move(ElecMap.Instance);
if (moveState == MoveType.move)
{
//int tPtr = vehicles[vnum].TPtr - 1;
//if (tPtr >= 0)
//{
// ElecMap.Instance.mapnode[vehicles[vnum].Route[tPtr].X, vehicles[vnum].Route[tPtr].Y].NodeCanUsed = -1;
//}
uint x = Convert.ToUInt32(vehicles[vnum].BeginX);
uint y = Convert.ToUInt32(vehicles[vnum].BeginY);
uint endX = Convert.ToUInt32(vehicles[vnum].EndX);
uint endY = Convert.ToUInt32(vehicles[vnum].EndY);
RunPacket rp = new RunPacket((byte)(serinum * vnum), (ushort)vnum, MoveDirection.Forward, 1500, new Destination(new CellPoint(x * ConstDefine.CELL_UNIT, y * ConstDefine.CELL_UNIT), new CellPoint(endX * ConstDefine.CELL_UNIT, endY * ConstDefine.CELL_UNIT), new AgvDriftAngle(0), TrayMotion.None));
AgvServerManager.Instance.SendTo(rp, vnum);
Console.WriteLine("*--------------------------------------------------------------------------*");
Console.WriteLine(vnum + ":" + x + "," + y + "->" + endX + "," + endY + " ,实际位置：" + vehicles[vnum].agvInfo.CurLocation.CurNode.X / 1000.0 + "," + vehicles[vnum].agvInfo.CurLocation.CurNode.Y / 1000.0 + "序列号：" + (serinum - 1));
CheckAlarmState(vnum);
vehicles[vnum].WaitEndTime = DateTime.Now.AddSeconds(WAIT_TIME);
ElecMap.Instance.mapnode[vehicles[vnum].BeginX, vehicles[vnum].BeginY].TraCongesIntensity = 100;
}
//if (moveState == MoveType.Swerve0)
//{
// SendSwerveCommand(vnum,0);
//}
//if (moveState == MoveType.Swerve90)
//{
// SendSwerveCommand(vnum, 90);
//}
//if (moveState == MoveType.Swerve180)
//{
// SendSwerveCommand(vnum, 180);
//}
//if (moveState == MoveType.Swerve270)
//{
// SendSwerveCommand(vnum, 270);
//}
#endif
moveCount++;
OnShowMessage(string.Format("{0:N} 公里", (moveCount * 1.5) / 1000.0));
}
}
if (vFinished != null)
{
for (int i = 0; i < vFinished.Count; i++)
{
FinishedQueue.Instance.Enqueue(vFinished[i]);
}
vFinished.Clear();
}
}
protected override void Run()
{
Thread.Sleep(ConstDefine.STEP_TIME);
if (vehicles == null)
{
return;
}
for (int vnum = 0; vnum < vehicles.Length; vnum++)
{
serinum = (byte)(serinum % 255);
if (vehicles[vnum].CurState == State.cannotToDestination && vehicles[vnum].Arrive == false)
{
vehicles[vnum].Arrive = true;
vFinished.Add(vehicles[vnum]);
vehicles[vnum].Route.Clear();
string str = string.Format("小车" + vnum + ":({0}，{1})->({2}，{3})没有搜索到路径，", vehicles[vnum].BeginX, vehicles[vnum].BeginY, vehicles[vnum].EndX, vehicles[vnum].EndY);
OnShowMessage(this, new MessageEventArgs(str));
continue;
}
if (vehicles[vnum].Route == null || vehicles[vnum].Route.Count < 1)
{
continue;
}
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.carried)
{
if (vehicles[vnum].EqualWithRealLocation(vehicles[vnum].BeginX, vehicles[vnum].BeginY))
{
if (vehicles[vnum].agvInfo.AgvMotion == AgvMotionState.StopedNode)
{
TrayPacket tp = new TrayPacket(serinum++, 4, TrayMotion.TopLeft);
// SendPacketQueue.Instance.Enqueue(tp);
AgvServerManager.Instance.Send(tp);
vehicles[vnum].CurState = State.unloading;
vehicles[vnum].WaitEndTime = DateTime.Now.AddSeconds(WAIT_TIME);
Console.WriteLine("send TrayMotion:" + (serinum - 1));
}
}
//else
//{
// uint x = Convert.ToUInt32(vehicles[vnum].BeginX);
// uint y = Convert.ToUInt32(vehicles[vnum].BeginY);
// uint endX = Convert.ToUInt32(vehicles[vnum].EndX);
// uint endY = Convert.ToUInt32(vehicles[vnum].EndY);
// RunPacket rp = new RunPacket(serinum, 4, MoveDirection.Forward, 1500, new Destination(new CellPoint(x * ConstDefine.CELL_UNIT, y * ConstDefine.CELL_UNIT), new CellPoint(endX * ConstDefine.CELL_UNIT, endY * ConstDefine.CELL_UNIT), new AgvDriftAngle(0), TrayMotion.None));
// //asm.Send(rp);
// SendPacketQueue.Instance.Enqueue(rp);
// Console.WriteLine("unloaing resend");
//}
continue;
}
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.unloading)
{
if (DateTime.Now > vehicles[vnum].WaitEndTime)
{
if (vehicles[vnum].EqualWithRealLocation(vehicles[vnum].BeginX, vehicles[vnum].BeginY))
{
if (vehicles[vnum].agvInfo.AgvMotion == AgvMotionState.StopedNode)
{
TrayPacket tp = new TrayPacket(serinum++, 4, TrayMotion.TopLeft);
// SendPacketQueue.Instance.Enqueue(tp);
AgvServerManager.Instance.Send(tp);
vehicles[vnum].CurState = State.unloading;
vehicles[vnum].WaitEndTime = DateTime.Now.AddSeconds(WAIT_TIME);
Console.WriteLine("resend TrayMotion**********:" + (serinum - 1));
}
}
}
continue;
}
if (vehicles[vnum].Arrive == true && vehicles[vnum].CurState == State.Free)
{
vFinished.Add(vehicles[vnum]);
vehicles[vnum].Route.Clear();
vehicles[vnum].LockNode.Clear();
Console.WriteLine("下一个目标：");
RandomMove(4);
continue;
}
if (vehicles[vnum].StopTime < 0)
{
if (vehicles[vnum].CurNodeTypy() != MapNodeType.queuingArea && GetDirCount(vehicles[vnum].BeginX, vehicles[vnum].BeginY) > 1)
{
if (vehicles[vnum].Stoped > -1 && vehicles[vnum].Stoped < vehicles.Length)
{
vehicles[vehicles[vnum].Stoped].StopTime = 2;
}
//重新搜索路径
SearchRoute(vnum, true);
}
vehicles[vnum].StopTime = 3;
}
else
{
bool isMove = vehicles[vnum].Move(ElecMap.Instance);// vehicles[vnum].SimpleMove();//
if (isMove)
{
// AGVServerManager.Instance.Send(MessageType.Move, vehicles[vnum].BeginX + ":" + vehicles[vnum].BeginY + ":"
//+ vehicles[vnum].EndX + ":" + vehicles[vnum].EndY);
uint x = Convert.ToUInt32(vehicles[vnum].BeginX);
uint y = Convert.ToUInt32(vehicles[vnum].BeginY);
uint endX = Convert.ToUInt32(vehicles[vnum].EndX);
uint endY = Convert.ToUInt32(vehicles[vnum].EndY);
RunPacket rp = new RunPacket(serinum++, 4, MoveDirection.Forward, 1500, new Destination(new CellPoint(x * ConstDefine.CELL_UNIT, y * ConstDefine.CELL_UNIT), new CellPoint(endX * ConstDefine.CELL_UNIT, endY * ConstDefine.CELL_UNIT), new AgvDriftAngle(0), TrayMotion.None));
AgvServerManager.Instance.Send(rp);
Console.WriteLine("*--------------------------------------------------------------------------*");
Console.WriteLine(vehicles[vnum].TPtr + ":" + x + "," + y + "->" + endX + "," + endY + " ,实际位置：" + vehicles[vnum].agvInfo.CurLocation.CurNode.X / 1000.0 + "," + vehicles[vnum].agvInfo.CurLocation.CurNode.Y / 1000.0 + "序列号：" + (serinum - 1));
CheckAlarmState(vnum);
vehicles[vnum].WaitEndTime = DateTime.Now.AddSeconds(WAIT_TIME);
moveCount++;
OnShowMessage(string.Format("{0:N} 公里", (moveCount * 1.5) / 1000.0));
}
}
}
if (vFinished != null)
{
for (int i = 0; i < vFinished.Count; i++)
{
FinishedQueue.Instance.Enqueue(vFinished[i]);
}
vFinished.Clear();
}
}
