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); //} }
private void button4_Click(object sender, EventArgs e) { try { uint len = 1000; TrayPacket tp = new TrayPacket(1, 4, TrayMotion.TopLeft); //RunPacket rp = new RunPacket(1, 4, MoveDirection.Forward, 1500, new Destination(new MyPoint(8 * len,5 * len), new MyPoint(8 * len, 5 * len), new AgvDriftAngle(90), TrayMotion.TopLeft)); asm.Send(tp); //for (int i = 0; i < route.Count; i++) //{ // RunPacket rp = new RunPacket(1, 4, AgvDirection.Forward, 1500, new Destination(new MyPoint(route[i].X * len, route[i].Y * len), new MyPoint(route[route.Count - 1].X * len, route[route.Count - 1].Y * len), new DriftAngle(90), TrayMotion.None)); // asm.Send(rp); //} //route.Reverse(); } catch (Exception ex) { MessageBox.Show("发送失败"); } }
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(); } }