public static Motion GetMotion(STask st, PathPoint ppCurrent)
{
Motion motion = new Motion();
motion.sTaskType = st.sTaskType;
if (st.sTaskType == STaskType.D1)
{
//如果是子任务的终点
if (ppCurrent.serialNo == st.pathList[st.pathList.Count - 1].serialNo)
{
motion.sTaskType = STaskType.D18;
}
//如果是拐点
if (ppCurrent.isCorner)
{
motion.sTaskType = STaskType.D20;
}
}
motion.barcode = ppCurrent.point.barCode;
motion.x = ppCurrent.point.x;
motion.y = ppCurrent.point.y;
motion.xLength = ppCurrent.point.xLength;
motion.yLength = ppCurrent.point.xLength;
motion.pointType = (int)ppCurrent.point.pointType;
return(motion);
}
/// <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>
/// <returns>路径集合</returns>
public static List <PathPoint> GetPath(Agv agv)
{
STask sTask = agv.sTaskList[0];
Point beginPoint = sTask.beginPoint;
Point endPoint = sTask.endPoint;
STaskType sTaskType = sTask.sTaskType;
switch (sTask.sTaskType)
{
case STaskType.D1:
if (beginPoint == endPoint)
{
return new List <PathPoint>()
{
GetObject.GetPathPoint(beginPoint), GetObject.GetPathPoint(endPoint)
}
}
;
AStar aStart = new AStar();
List <Point> listApoint = GetCanUseApoint(agv);
ClearParentPoint();
Point Parent = aStart.PathGet(listApoint, beginPoint, endPoint, agv);
if (Parent == null)
{
return(null);
}
List <PathPoint> listPathPoint = GetPathList(Parent);
AddCorner(listPathPoint);
OriLock(listPathPoint, agv);
return(listPathPoint);
case STaskType.D2:
case STaskType.D3:
case STaskType.D6:
case STaskType.D15:
return(new List <PathPoint>()
{
GetObject.GetPathPoint(beginPoint), GetObject.GetPathPoint(endPoint)
});
case STaskType.D13:
case STaskType.D14:
//左弧右弧进
break;
case STaskType.D17:
case STaskType.D19:
//左弧右弧出
break;
default:
throw new Exception("无此任务类型:" + sTask.sTaskType);
}
return(null);
}
/// <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>
public static void FinishTask(Agv agv, STask sTask)
{
SqlParameter[] para = new SqlParameter[1];
para[0] = new SqlParameter("SID", SqlDbType.Int);
para[0].Value = sTask.sID;
DbHelperSQL.RunProc("P_Task_Finish", para);
agv.sTaskList.Remove(sTask);
}
/// <summary>
/// 执行任务
/// </summary>
/// <param name="sTask"></param>
public static void DoingTask(STask sTask)
{
//更新数据库发送的子任务状态
string sql = string.Format(@"UPDATE dbo.T_Task_Son SET State = '3' WHERE SID = '{0}'", sTask.sID);
DbHelperSQL.ExecuteSql(sql);
sTask.state = TaskState.Doing;
}
public static void ResetPath(Agv agv)
{
//删除当前路径
//任务状态变为1
STask sTask = agv.sTaskList[0];
string sql = string.Format("DELETE FROM dbo.T_Base_PathList WHERE SID = '{0}';", sTask.sID);
sql += string.Format("UPDATE dbo.T_Task_Son SET State = 1 WHERE SID = '{0}' ;", sTask.sID);
DbHelperSQL.ExecuteSql(sql);
sTask.pathList.Clear();
sTask.state = TaskState.Down;
}
public static STask GetSTask(DataRow dr)
{
STask sTask = new STask();
sTask.serialNo = int.Parse(dr["serialNo"].ToString());;
sTask.sID = int.Parse(dr["SID"].ToString());
sTask.taskNo = dr["TaskNo"].ToString();
sTask.sTaskType = (STaskType)int.Parse(dr["ItemName"].ToString());
sTask.agv = App.AgvList.FirstOrDefault(a => a.agvNo == dr["AgvNo"].ToString());
sTask.beginPoint = App.pointList.FirstOrDefault(a => a.barCode == dr["BeginPoint"].ToString());
sTask.endPoint = App.pointList.FirstOrDefault(a => a.barCode == dr["EndPoint"].ToString());
sTask.dialDirection = int.Parse(dr["DialDirection"].ToString());
sTask.agvDirection = int.Parse(dr["AgvDirection"].ToString());
sTask.state = (TaskState)(int.Parse(dr["State"].ToString()));
sTask.pathList = new List <PathPoint>();
sTask.agv.sTaskList.Add(sTask);
return(sTask);
}
/// <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);
}
static void DownPath()
{
string sql = "SELECT * FROM dbo.T_Base_PathList order by sid,SerialNo";
DataSet ds = DbHelperSQL.Query(sql);
List <PathPoint> listPathPoint = new List <PathPoint>();
foreach (DataRow dr in ds.Tables[0].Rows)
{
int sID = int.Parse(dr["SID"].ToString());
string barcode = dr["Barcode"].ToString();
int serialNo = int.Parse(dr["serialNo"].ToString());
int direction = int.Parse(dr["Direction"].ToString());
PathPoint pp = new PathPoint();
pp.SID = sID;
pp.serialNo = serialNo;
pp.point = App.pointList.FirstOrDefault(a => a.barCode == barcode);
listPathPoint.Add(pp);
}
foreach (Agv agv in App.AgvList)
{
if (agv.sTaskList.Count == 0)
{
continue;
}
//将此点放到对应子任务的路径中
STask sTask = agv.sTaskList[0];
if (listPathPoint.Exists(a => a.SID == sTask.sID))
{
sTask.pathList = listPathPoint.FindAll(a => a.SID == sTask.sID);
sTask.pathList.Sort();
PathGet.OriLock(sTask.pathList, agv);
}
}
}
/// <summary>
/// 下载任务
/// </summary>
static void DownTask()
{
try
{
string sql = @"SELECT ts.*,c.ItemName FROM dbo.T_Task_Son ts
LEFT JOIN dbo.T_Type_Config c ON ts.STaskType = c.ItemNo
WHERE State = 0 ORDER BY AgvNo, SerialNo";
DataSet ds = DbHelperSQL.Query(sql);
DataRowCollection drc = ds.Tables[0].Rows;
foreach (DataRow dr in drc)
{
STask s = GetObject.GetSTask(dr);
string sql2 = string.Format("UPDATE dbo.T_Task_Son SET State = 1 WHERE SID = '{0}'", s.sID);
DbHelperSQL.ExecuteSql(sql2);
s.state = TaskState.Down;
}
}
catch (Exception ex)
{
App.ExFile.MessageLog("DownTask", ex.Message + "\r");
}
}
/// <summary>
/// 获取行走后面的点
/// </summary>
static List <Motion> GetNextListMotion(STask sTask, PathPoint pathPoint, Agv agv)
{
List <Motion> listMotion = new List <Motion>();
List <PathPoint> listPathPoint = sTask.pathList;
//移除当前点之前的路径点
sTask.pathList.RemoveAll(a => a.serialNo < pathPoint.serialNo);
int i = 0;
foreach (PathPoint pp in listPathPoint)
{
i++;
if (i == 5)
{
break;
}
Point nowPoint = pp.point;
if (Commond.IsTest && nowPoint.barCode == "2280")
{
listMotion.Add(GetObject.GetMotion(sTask, pp));
continue;
}
if (nowPoint.lockedAgv == agv)
{
listMotion.Add(GetObject.GetMotion(sTask, pp));
continue;
}
if (nowPoint.lockedAgv != null)
{
//找到占用此点的小车
//如果小车没任务且在低位，则让其回家
Agv nextAgv = nowPoint.lockedAgv;
if (nextAgv.sTaskList.Count == 0)
{
//创建一个回家任务
}
else
{
List <PathPoint> lpp = nextAgv.sTaskList[0].pathList;
if (lpp.Exists(a => a.point.barCode == agv.barcode))
{
//发送取消任务
//agv回复取消成果，则删除路径及更新任务状态
//Task.ResetPath(agv);
}
}
break;
}
if (IsLoop(nowPoint, new List <Agv>()
{
agv
}))
{
break;
}
//如果当前点货架，且小车是顶升 不走
Shelf shelf = App.ShelfList.FirstOrDefault(a => a.currentBarcode == nowPoint.barCode);
if (agv.height != HeightEnum.Low && shelf != null)
{
break;
}
nowPoint.lockedAgv = agv;
listMotion.Add(GetObject.GetMotion(sTask, pp));
if (pp.isCorner)
{
break;
}
}
return(listMotion);
}
/// <summary>
/// 分配路径
/// </summary>
static void AssignePath()
{
//try
//{
//遍历所有AGV，找agv中的第一条任务
//如果状态是已下载，则分配路径
//如果分配成功，则更新数据库任务状态，插入路径
//如果更新成功，则更新内存已分配路径,更新任务状态
foreach (Agv agv in App.AgvList)
{
List <STask> listSTask = agv.sTaskList;
if (listSTask.Count == 0)
{
continue;
}
STask sTask = listSTask[0];
if (sTask.state != TaskState.Down)
{
continue;
}
string sql = "";
List <PathPoint> listPathPoint = new List <PathPoint>();
//如果起点和终点一致，且是行走，则直接结束任务
if (sTask.beginPoint == sTask.endPoint && sTask.sTaskType == STaskType.D1)
{
FinishTask(agv, sTask);
continue;
}
//因未实现路径重建，临时使用此方式来避免 将任务创建在别的小车路径上
//如果不是最后一个任务且是行走，如果终点被锁，则先不查找路径
if (listSTask.Count > 1 && sTask.sTaskType == STaskType.D1)
{
if (sTask.endPoint.listTmpDirection.Count != 0)
{
continue;
}
}
if (sTask.sTaskType == STaskType.D25)
{
FinishTask(agv, sTask);
continue;
}
listPathPoint = PathGet.GetPath(agv);
if (listPathPoint == null)
{
continue;
}
int serialNo = 1;
foreach (PathPoint pp in listPathPoint)
{
//待修改，需将点的方向也丢到路径中
sql += string.Format("INSERT INTO T_Base_PathList (SID,Barcode,serialNo,direction) VALUES('{0}','{1}','{2}','{3}');", agv.sTaskList[0].sID, pp.point.barCode, serialNo, 0);
serialNo++;
}
sql += string.Format("UPDATE dbo.T_Task_Son SET State = 2 WHERE SID = '{0}'", sTask.sID);
DbHelperSQL.ExecuteSql(sql);
sTask.pathList = listPathPoint;
sTask.state = TaskState.HavePath;
}
//}
//catch (Exception ex)
//{
// App.ExFile.MessageLog("AssignePath", ex.Message + "\r");
//}
}
/// <summary>
///获取可用的路径点
/// </summary>
static List <Point> GetCanUseApoint(Agv agv)
{
STask sTask = agv.sTaskList[0];
Point beginPoint = sTask.beginPoint;
Point endPoint = sTask.endPoint;
//获取可用点
List <Point> listApoint = new List <Point>();
foreach (Point point in App.pointList)
{
if (point != beginPoint && point != endPoint)
{
//如果是货架点
if (point.pointType == PointType.D2)
{
//如果是顶升，则禁止使用货架点
if (agv.height == HeightEnum.High)
{
continue;
}
//如果是其他任务的起点/终点，则禁止通过
bool isTask = App.AgvList.Exists(a =>
a.sTaskList.Exists(b =>
b.beginPoint == point || b.endPoint == point));
if (isTask)
{
continue;
}
}
//以下点类型不使用
if (point.pointType == PointType.D4 ||
point.pointType == PointType.D6 ||
point.pointType == PointType.D8 ||
point.pointType == PointType.D9 ||
point.pointType == PointType.D10 ||
point.pointType == PointType.D11 ||
point.pointType == PointType.D14 ||
point.pointType == PointType.D15
)
{
continue;
}
//被占用且不是终点，不使用
if (point.isOccupy)
{
continue;
}
//异常车上的点不申请
if (point.lockedAgv != null && point.lockedAgv.errorMsg != 0)
{
continue;
}
//不包含agv的区域，则不选择
if (!point.areaNo.Contains(agv.areaNo))
{
continue;
}
}
listApoint.Add(point);
}
return(listApoint);
}
