/// <summary> /// 更新点方向锁信息 /// </summary> static void UpdataPointOri(AgvMsg agvMsg, Agv agv) { //找到小车当前的任务 STask sTask = agv.sTaskList.FirstOrDefault(a => a.sID == agvMsg.SID); if (sTask == null) { return; } PathPoint pathCurrent = sTask.pathList.FirstOrDefault(a => a.point.barCode == agv.barcode); if (pathCurrent == null) { throw new Exception("AGV当前点不在路径中!" + agv.agvNo); } //查找所有被当前小车锁定的点 List <Point> listPoint = App.pointList.Where(a => a.listTmpDirection.Exists(b => b.agvNo == agv.agvNo)).ToList(); foreach (Point point in listPoint) { //如果当前点不在小车的路径中 //如果当前点是小车所在的点,当前点不能解除方向锁。解除会导致对面的车冲突 //则清理当前点的路径锁 if (!sTask.pathList.Exists(a => a.point == point)) { point.listTmpDirection.RemoveAll(a => a.agvNo == agv.agvNo); } } }
/// <summary> /// 接收小车动作完成操作 /// </summary> static void AgvDoTask(AgvMsg agvMsg, SocketOpt socketOpt) { Agv agv = socketOpt.agv; STask sTask = agv.sTaskList.FirstOrDefault(a => a.sID == agvMsg.SID); if (sTask == null) { throw new Exception("无法找到子任务!"); } PathPoint pPoint = sTask.pathList.FirstOrDefault(a => a.point.barCode == agvMsg.Barcode); switch (agvMsg.sTaskType) { case STaskType.D1: case STaskType.D20: case STaskType.D21: case STaskType.D24: List <Motion> listMotion = GetNextListMotion(sTask, pPoint, agv); if (listMotion.Count == 0) { throw new Exception("找到0条路径"); } else if (listMotion.Count == 1) { socketOpt.Data = SendData.GetRepeatData(agv.agvNo); } else { socketOpt.Data = SendData.GetNewActionData(agvMsg, listMotion); } //除发送给AGV的点外,其它点全部解锁方向 NoLock(agv, listMotion); break; case STaskType.D2: case STaskType.D3: case STaskType.D6: case STaskType.D15: case STaskType.D18: case STaskType.D22: case STaskType.D23: Task.FinishTask(agv, sTask); socketOpt.Data = SendData.GetFinishData(agv.agvNo); break; case STaskType.D25: //if (beginPoint.barCode != endPoint.barCode) throw new Exception("原地旋转,起点和终点必须一致!"); break; default: throw new Exception("未找到此任务类型"); } ManageTcp.Send(socketOpt); }
/// <summary> /// 更新小车货架信息 /// </summary> static void UpdateAgvShelfPoint(AgvMsg agvMsg, Agv agv) { string oldBarcode = agv.barcode; string newBarcode = agvMsg.Barcode; //刚开机,没扫到码,给的是0 if (newBarcode == "0") { agvMsg.Barcode = newBarcode = oldBarcode; } UpdatePoint(oldBarcode, newBarcode, agv); ////如果AGV的位置状态不变,不更新Agv状态 if (newBarcode == oldBarcode && agv.height == agvMsg.Height && agv.currentCharge == agvMsg.Voltage && agv.state == (AgvState)agvMsg.State && agv.errorMsg == agvMsg.ErrCode1 + agvMsg.ErrCode2 + agvMsg.ErrCode3) { return; } //配送与仓库不同,此判断条件需要考量。 低位/中位/高位的问题 if (agvMsg.Height == HeightEnum.High) { Shelf shelf = App.ShelfList.FirstOrDefault(a => a.currentBarcode == oldBarcode); if (shelf != null) { //货架的当前点要变更为现在的点 string shelfSql = string.Format(@"UPDATE T_Base_Shelf SET CurrentBarcode = '{0}' WHERE CurrentBarcode = '{1}';", newBarcode, oldBarcode); DbHelperSQL.ExecuteSql(shelfSql); shelf.currentBarcode = newBarcode; } else { throw new Exception("小车顶升是高位,但是无货架点!"); } } //更新数据库中小车的状态 int error = agvMsg.ErrCode1 * 10000 + agvMsg.ErrCode2 * 100 + agvMsg.ErrCode3; string sql = string.Format(@"UPDATE dbo.T_Base_AGV SET Barcode = '{0}',ErrorMsg = '{1}', Height = '{2}',CurrentCharge = '{3}',UpdateTime = GETDATE(),State = '{4}' WHERE AgvNo = '{5}'", newBarcode, error, (int)agvMsg.Height, agvMsg.Voltage, agvMsg.State, agvMsg.AgvNo); DbHelperSQL.ExecuteSql(sql); agv.barcode = newBarcode; agv.height = agvMsg.Height; agv.currentCharge = agvMsg.Voltage; agv.state = (AgvState)agvMsg.State; agv.errorMsg = agvMsg.ErrCode1 + agvMsg.ErrCode2 + agvMsg.ErrCode3; }
/// <summary> /// 更新数据 /// </summary> static void SendTask(AgvMsg agvMsg, SocketOpt socketOpt) { Agv agv = socketOpt.agv; if (agv.state != AgvState.D11 && agv.state != AgvState.D12) { return; } if (agv.errorMsg != 0 && agv.errorMsg != 19 && agv.errorMsg != 1900 && agv.errorMsg != 190000) { return; } if (agv.sTaskList.Count == 0) { return; } STask sTask = agv.sTaskList[0]; if (sTask.state != TaskState.HavePath) { return; } byte SOri, EOri; if (sTask.dialDirection == 1) { SOri = 1; } else { SOri = 0; } EOri = (byte)sTask.dialDirection; socketOpt.Data = SendData.GetNewTaskData(agvMsg.AgvNo, sTask.sID, SOri, EOri); ManageTcp.Send(socketOpt); Task.DoingTask(sTask); }
/// <summary> /// 指令解析 /// </summary> /// <param name="socketOpt">自定义Socket对象</param> public static void DataTranslate(SocketOpt socketOpt) { byte[] DataRecv = socketOpt.Data; string AgvNo = Encoding.ASCII.GetString(DataRecv, 3, 10).Replace("\0", ""); //try //{ AgvMsg agvMsg = new AgvMsg(); int DataLength = DataRecv[2]; agvMsg.AgvNo = Encoding.ASCII.GetString(DataRecv, 3, 10).Replace("\0", ""); socketOpt.agv = App.AgvList.FirstOrDefault(a => a.agvNo == agvMsg.AgvNo); if (socketOpt.agv == null) { throw new Exception("更新状态失败:找不到小车号。"); } agvMsg.Barcode = BitConverter.ToUInt32(DataRecv, 13).ToString(); agvMsg.Voltage = BitConverter.ToInt16(DataRecv, 17) / 100; agvMsg.State = DataRecv[19]; agvMsg.Height = (HeightEnum)DataRecv[20]; agvMsg.ErrCode1 = DataRecv[21]; agvMsg.ErrCode2 = DataRecv[22]; agvMsg.ErrCode3 = DataRecv[23]; string sid = Encoding.ASCII.GetString(DataRecv, 26, 20).Replace("\0", ""); if (string.IsNullOrEmpty(sid)) { agvMsg.SID = 0; } else { agvMsg.SID = int.Parse(sid); } agvMsg.sTaskType = (STaskType)DataRecv[46]; int CrcR = DataRecv[47]; int CrcC = Commond.CRC(DataRecv, DataLength + 14); if (CrcC == CrcR) { Agv agv = socketOpt.agv; UpdateAgvShelfPoint(agvMsg, agv); if (agvMsg.SID == 0) { SendTask(agvMsg, socketOpt); } else { UpdataPointOri(agvMsg, agv); AgvDoTask(agvMsg, socketOpt); } } //} //catch (Exception ex) //{ // App.ExFile.MessageLog("DataTranslate", ex.Message + "\r" // + Encoding.Default.GetString(DataRecv)); //} socketOpt.Data = SendData.GetRepeatData(AgvNo); ManageTcp.Send(socketOpt); }
/// <summary> /// 获取新动作 /// </summary> /// <param name="agvMsg">小车信息</param> /// <param name="listMotion">动作列表</param> /// <returns></returns> public static byte[] GetNewActionData(AgvMsg agvMsg, List <Motion> listMotion) { byte[] Data = new byte[89]; Data[0] = 0; Data[1] = 3; //功能码 Data[2] = 75; byte[] DataAdress = Encoding.ASCII.GetBytes(agvMsg.AgvNo); for (int index = 3; index < 3 + DataAdress.Length; index++) { Data[index] = DataAdress[index - 3]; } //动作报文拼接 int DTypeIndex = 0; foreach (Motion newMotion in listMotion) { byte[] BarCode1 = new byte[4]; byte[] DXpos = new byte[2]; byte[] DYpos = new byte[2]; byte[] DXdis = new byte[2]; byte[] DYdis = new byte[2]; Data[13 + DTypeIndex] = (byte)newMotion.sTaskType; //动作类型 UInt32 barCode1 = UInt32.Parse(newMotion.barcode); //码值 BarCode1 = BitConverter.GetBytes(barCode1); Data[14 + DTypeIndex] = BarCode1[0]; Data[15 + DTypeIndex] = BarCode1[1]; Data[16 + DTypeIndex] = BarCode1[2]; Data[17 + DTypeIndex] = BarCode1[3]; Int16 xpos = (Int16)newMotion.x; //X坐标 DXpos = BitConverter.GetBytes(xpos); Data[18 + DTypeIndex] = DXpos[0]; Data[19 + DTypeIndex] = DXpos[1]; Int16 ypos = (Int16)newMotion.y; //Y坐标 DYpos = BitConverter.GetBytes(ypos); Data[20 + DTypeIndex] = DYpos[0]; Data[21 + DTypeIndex] = DYpos[1]; Int16 xdis = (Int16)newMotion.xLength; //X间距 DXdis = BitConverter.GetBytes(xdis); Data[22 + DTypeIndex] = DXdis[0]; Data[23 + DTypeIndex] = DXdis[1]; Int16 ydis = (Int16)newMotion.yLength; //Y间距 DYdis = BitConverter.GetBytes(xdis); Data[24 + DTypeIndex] = DYdis[0]; Data[25 + DTypeIndex] = DYdis[1]; Data[26 + DTypeIndex] = (byte)newMotion.pointType; //点属性 Data[27 + DTypeIndex] = 1; //托盘属性 是否需要?? DTypeIndex = DTypeIndex + 15; } Data[88] = Commond.CRC(Data, 87); string s = Encoding.ASCII.GetString(Data, 0, Data.Length); App.ExFile.MessageLog("DataTranslate", s + "\r"); return(Data); }