/// <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);
}
