/// <summary>
/// 计算路线中拐弯数量
/// </summary>
private int AcountInflect(List <LandmarkInfo> route)
{
try
{
int Count = 0;
LandmarkInfo UpLand = null;
LandmarkInfo InflectLand = null;
LandmarkInfo NextLand = null;
if (route.Count >= 3)
{
for (int i = 2; i < route.Count; i++)
{
UpLand = route[i - 2];
InflectLand = route[i - 1];
NextLand = route[i];
double SP = (Math.Round(UpLand.LandX, 3) - Math.Round(NextLand.LandX, 3)) * (Math.Round(InflectLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) - (Math.Round(UpLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) * (Math.Round(InflectLand.LandX, 3) - Math.Round(NextLand.LandX, 3));
if (SP > 0.05 || SP < -0.05)
{
Count += 1;
}
}
}
return(Count);
}
catch (Exception ex)
{ throw ex; }
}
public void Setup(LandmarkInfo info)
{
text.text = (info.displayPhrase.IsValid() ? info.displayPhrase.translated : info.transform.root.name);
if (info.mapIcon != null)
{
image.sprite = info.mapIcon;
text.SetActive(false);
imageBackground.SetActive(true);
}
else
{
text.SetActive(true);
imageBackground.SetActive(false);
}
SetNightMode(false);
}
// Use this for initialization
void Start()
{
if (!started)
{
items = items.GetComponent <Canvas>();
try
{
//DontDestroyOnLoad(gameObject);
//remote endpoint of the server
IPEndPoint remoteEP = new IPEndPoint(IP, GC_PORT);
//create TCP socket
client = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
//connect to remote endpoint
client.Connect(remoteEP);
//send the players id to the server
client.Send(Player.playerID.ToByteArray());
Debug.Log("Connect GenCom Successful");
update = new byte[UPDATE_SIZE];
update[0] = 255;
ackReceived = false;
type = new byte[1];
landmarkInfo = new LandmarkInfo();
colloseumInfo = new ColloseumInfo();
battleInfo = new BattleInfo();
processed = new ManualResetEvent(true);
//Debug.Log("Send Update Successful");
//start receiving updates from server
client.BeginReceive(type, 0, 1, 0, new AsyncCallback(updateDriver), null);
//Debug.Log("Start async successful");
}
//catch exception if fail to connect
catch (Exception e)
{
Debug.Log(e.ToString());
Debug.Log("Connection Failure");
}
}
}
/// <summary>
/// 得到下一个地标
/// </summary>
/// <returns></returns>
private List <LandmarkInfo> getNextLand(LandmarkInfo PreLand)
{
try
{
List <LandmarkInfo> lands = new List <LandmarkInfo>();
List <SegmentResInfo> Children = Segments.Where(p => p.StartLandMark.LandmarkCode == PreLand.LandmarkCode).ToList();
foreach (SegmentResInfo item in Children)
{
if (CloseList.Where(p => p.LandmarkCode == item.EndLandMark.LandmarkCode).Count() <= 0)
{
lands.Add(item.EndLandMark);
}
}
return(lands);
}
catch (Exception ex)
{ throw ex; }
}
/// <summary>
/// 得到下一个地标
/// </summary>
/// <returns></returns>
private List <LandmarkInfo> getNextLand(LandmarkInfo PreLand)
{
try
{
List <LandmarkInfo> lands = new List <LandmarkInfo>();
List <AllSegment> Children = Segments.Where(p => p.BeginLandMakCode == PreLand.LandmarkCode).ToList();
foreach (AllSegment item in Children)
{
if (CloseList.Where(p => p.LandmarkCode == item.EndLandMarkCode).Count() <= 0)
{
LandmarkInfo EndLandMark = AllLands.FirstOrDefault(p => p.LandmarkCode == item.EndLandMarkCode);
lands.Add(EndLandMark);
}
}
return(lands);
}
catch (Exception ex)
{ throw ex; }
}
/// <summary>
/// 得到路线分支中的令一个地标
/// </summary>
/// <param name="Route"></param>
/// <param name="PreLand"></param>
/// <returns></returns>
private List <LandmarkInfo> getRouteEmbranchmentAnothertLand(List <LandmarkInfo> Route, LandmarkInfo PreLand)
{
try
{
List <LandmarkInfo> lands = new List <LandmarkInfo>();
List <AllSegment> Children = Segments.Where(p => p.BeginLandMakCode == PreLand.LandmarkCode).ToList();
foreach (AllSegment item in Children)
{
if (Route.Where(p => p.LandmarkCode == item.EndLandMarkCode).Count() <= 0)
{
LandmarkInfo EndLandMark = AllLands.FirstOrDefault(p => p.LandmarkCode == item.EndLandMarkCode);
lands.Add(EndLandMark);
}
}
return(lands);
}
catch (Exception ex)
{ LogHelper.WriteErrorLog(ex); return(new List <LandmarkInfo>()); }
}
/// <summary>
/// 得到路线的地标集合
/// </summary>
public List <string> GetRouteStr(List <LandmarkInfo> route)
{
try
{
List <LandmarkInfo> CopyRoutes = CreateDeepCopy <List <LandmarkInfo> >(route);
List <LandmarkInfo> ResultRoute = CreateDeepCopy <List <LandmarkInfo> >(route);
foreach (LandmarkInfo item in CopyRoutes)
{
if (item.sway != SwayEnum.None)
{
List <LandmarkInfo> CloseLand = (from a in AllLands
where a.LandmarkCode != item.LandmarkCode
orderby getDistant(a.LandX, a.LandY, item.LandX, item.LandY) ascending
select a).ToList();
int IndexItem = ResultRoute.FindIndex(p => p.LandmarkCode == item.LandmarkCode);
int Count = 1;
int InsetCount = 1;
foreach (LandmarkInfo Copy in CloseLand)
{
if (Count == 5)
{
break;
}
if (ResultRoute.Where(p => p.LandmarkCode == Copy.LandmarkCode).Count() <= 0)
{
LandmarkInfo newLand = CreateDeepCopy <LandmarkInfo>(Copy);
ResultRoute.Insert(IndexItem + InsetCount, newLand);
InsetCount++;
}
Count++;
}
}
}
string RouteStr = string.Join(",", ResultRoute.Distinct().Select(p => p.LandmarkCode));
List <string> ResultStr = RouteStr.Split(',').ToList <string>();
//string dir = Environment.GetFolderPath(Environment.SpecialFolder.DesktopDirectory);
//File.AppendAllText(dir + "//RouteStr.txt", RouteStr);
return(ResultStr);
}
catch (Exception ex)
{ throw ex; }
}
/// <summary>
/// 加载agv的当前路线
/// </summary>
public static List <LandmarkInfo> LoadCarRoute(CarInfo Car)
{
try
{
IDbOperator dbOperator = CreateDbOperator.DbOperatorInstance(ConnectConfigTool.DBase);
List <LandmarkInfo> route = new List <LandmarkInfo>();
Hashtable dic = new Hashtable();
dic["AGVID"] = Car.AgvID;
DataTable dt = dbOperator.LoadDatas("QueryAGVRoute", dic);
if (dt != null && dt.Rows.Count > 0)
{
string RouteLandCodes = dt.Rows[0]["RouteLandCodes"].ToString();
if (string.IsNullOrEmpty(RouteLandCodes))
{
return(route);
}
else
{
string[] LandCodes = RouteLandCodes.Split(',');
foreach (string item in LandCodes)
{
LandmarkInfo Land = CoreData.AllLands.FirstOrDefault(p => p.LandmarkCode == item);
if (Land == null)
{
continue;
}
else
{
route.Add(ConnectConfigTool.CreateDeepCopy <LandmarkInfo>(Land));
}
}
}
return(route);
}
else
{
return(route);
}
}
catch (Exception ex)
{ throw ex; }
}
/// <summary>
/// 计算两点之间距离
/// </summary>
/// <returns></returns>
private double getDistant(string CurrLandCode, string TaskDetailLandCode)
{
try
{
LandmarkInfo land1 = AllLands.FirstOrDefault(p => p.LandmarkCode == CurrLandCode);
LandmarkInfo land2 = AllLands.FirstOrDefault(p => p.LandmarkCode == TaskDetailLandCode);
if (land1 != null && land2 != null)
{
double distant = Math.Sqrt(Math.Pow(Math.Abs(land1.LandX - land2.LandX), 2) + Math.Pow(Math.Abs(land1.LandY - land2.LandY), 2));
return(distant);
}
else
{
LogHelper.WriteLog("领取任务时查找直线最近车辆时找不到车当前地标或任务明细的目的地标");
}
}
catch (Exception ex)
{ LogHelper.WriteErrorLog(ex); }
return(-1);
}
public CarMonitor()
{
IsLock = false;
Sate = 0;
CurrLand = new LandmarkInfo();
NextLand = new LandmarkInfo();
CurrRoute = new List <LandmarkInfo>();
RouteLands = new List <string>();
TurnLands = new List <string>();
StandbyLandMark = "";
ExcuteTaksNo = "";
ArmLand = "";
Speed = 0.02;
CurrSite = -1;
CarActive_Timer = new System.Timers.Timer()
{
Enabled = false
};
CarActive_Timer.Elapsed += new System.Timers.ElapsedEventHandler(CarActive);
CarActive_Timer.Interval = 50;
}
public bool Inital()
{
try
{
MoniCars.Clear();
AllCar = AGVClientDAccess.LoadAGVAchive();
AllLands = AGVClientDAccess.LoadLandByCondition("1=1");
AllSegs = AGVClientDAccess.LoadAllSegment();
SimulatorVar.AllSegs = AllSegs;
Stores = AGVClientDAccess.LoadStorages();
System = AGVSimulationDAccess.LoadSystem();
foreach (CarBaseStateInfo item in AllCar)
{
CarMonitor moniCar = new CarMonitor();
moniCar.AgvID = item.AgvID;
moniCar.CurrSite = Convert.ToInt32(item.StandbyLandMark);
moniCar.StandbyLandMark = item.StandbyLandMark;
double ScalingRate = 0;
string ScalingRateStr = System["ScalingRate"].ToString();
try
{
ScalingRate = Convert.ToDouble(ScalingRateStr);
}
catch
{ }
if (ScalingRate > 0)
{
LandmarkInfo CurrLand = AllLands.FirstOrDefault(p => p.LandmarkCode == item.StandbyLandMark);
if (CurrLand != null)
{
moniCar.X = (float)(CurrLand.LandX * ScalingRate);
moniCar.Y = (float)(CurrLand.LandY * ScalingRate);
}
}
moniCar.ScalingRate = ScalingRate;
MoniCars.Add(moniCar);
}
if (Car_Ini != null)
{
Car_Ini(MoniCars);
}
//Juncts.Clear();
//IList<TrafficController> Traffics = AGVClientDAccess.GetTraffics();
//foreach (TrafficController item in Traffics)
//{
// JunctionInfo junct = new JunctionInfo();
// foreach (string s in item.EnterLandCode.Split(','))
// { junct.JunctionLandMarkCodes.Add(s); }
// junct.JunctionID = item.JunctionID;
// foreach (string s in item.JunctionLandMarkCodes.Split(','))
// { junct.JunctionLandMarkCodes.Add(s); }
// junct.RealseLandMarkCode = item.RealseLandMarkCode;
// Juncts.Add(junct);
//}
CountRoute = new RoutePlanData(AllSegs);
Traffic = new TrafficController(MoniCars, AllSegs, System, AllLands);
timerStarBeStopedCar.Enabled = true;
timerStarBeStopedCar.AutoReset = true;
timerStarBeStopedCar.Elapsed += TimerStarBeStopedCar_Elapsed;
timerFreshTask.Enabled = true;
timerFreshTask.AutoReset = true;
timerFreshTask.Elapsed += TimerFreshTask_Elapsed;
return(true);
}
catch (Exception ex)
{ return(false); throw ex; }
}
public void RepairRoute()
{
try
{
bool isRepair = false;
AcountDirect(ref RouteList);
List <LandmarkInfo> OrialRoute = CreateDeepCopy <List <LandmarkInfo> >(RouteList);
//找到计算的路线中的所有有分支的地标
List <LandmarkInfo> InflatLands = OrialRoute.Where(p => p.sway != SwayEnum.None).ToList();
for (int i = 0; i < InflatLands.Count; i++)
{
for (int j = i + 1; j < InflatLands.Count; j++)
{
LandmarkInfo FirstLand = InflatLands[i];
LandmarkInfo SecondLand = InflatLands[j];
if (FirstLand != null && SecondLand != null)
{
//重现计算这两个分支点之间的路径
//然后和开始计算的路径中的这两个地标之间的地标个数进行比较
//如果新算出来的比之前两个地标之间的地标个数少的话那么就用
//新算出来的路径片段替换原来的线路
//提出这两个地标之间在原来线路中的路径地标
List <LandmarkInfo> FindOldRouteFeg = new List <LandmarkInfo>();
int FirstIndex = OrialRoute.FindIndex(p => p.LandmarkCode == FirstLand.LandmarkCode);
int SecondIndex = OrialRoute.FindIndex(p => p.LandmarkCode == SecondLand.LandmarkCode);
if (FirstIndex >= 0 && SecondIndex >= 0)
{
for (int k = FirstIndex; k <= SecondIndex; k++)
{
FindOldRouteFeg.Add(CreateDeepCopy <LandmarkInfo>(OrialRoute[k]));
}
}
//重新计算两交叉地标之间的路径
CloseList.Clear();
RouteList.Clear();
AcountRoute(FirstLand, SecondLand);
//如果 新计算出来的两地标之间路径地标个数小于原先的那么则拿新算的路径地标信息替换原来的路径里面去
if (RouteList.Count > 0 && FindOldRouteFeg.Count > 0 && RouteList.Count < FindOldRouteFeg.Count)
{
OrialRoute.RemoveRange(FirstIndex, SecondIndex - FirstIndex + 1);
//再把新算的路径塞进去
OrialRoute.InsertRange(FirstIndex, RouteList);
isRepair = true;
break;
}
}
}
if (isRepair)
{
break;
}
}
if (isRepair)
{
RouteList = CreateDeepCopy <List <LandmarkInfo> >(OrialRoute);
RepairRoute();
}
else
{
RouteList = CreateDeepCopy <List <LandmarkInfo> >(OrialRoute);
}
}
catch (Exception ex)
{ LogHelper.WriteErrorLog(ex); }
}
//计算转向和前进和后退
public void AcountDirect(ref List <LandmarkInfo> Routes)
{
try
{
if (Routes.Count <= 1)
{
return;
}
IList <AllSegment> AllSegs = AGVClientDAccess.LoadAllSegment();
DataTable dtCoor = AGVClientDAccess.LoadAGVCoordinate();
LandmarkInfo UpLand = null;
LandmarkInfo InflectLand = null;
LandmarkInfo NextLand = null;
int IsCountAngel = 0;
SysParameter sys = AGVClientDAccess.GetParameterByCode("CountTurnType");
if (sys != null && sys.ParameterValue == "角度")
{
IsCountAngel = 1;
}
if (Routes.Count >= 3)
{
for (int i = 2; i < Routes.Count; i++)
{
UpLand = Routes[i - 2];
InflectLand = Routes[i - 1];
NextLand = Routes[i];
if (i != Routes.Count - 1)
{
#region 计算转向
if (IsCountAngel == 0)
{
AllSegment Segment = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (Segment != null && Segment.ExcuteTurnDirect != -1)
{
UpLand.sway = (SwayEnum)Segment.ExcuteTurnDirect;
}
else
{
if (AllSegs.Count(p => p.BeginLandMakCode == UpLand.LandmarkCode) > 1)
{
//double SP = (Math.Round(UpLand.LandX, 3) - Math.Round(NextLand.LandX, 3)) * (Math.Round(InflectLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) - (Math.Round(UpLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) * (Math.Round(InflectLand.LandX, 3) - Math.Round(NextLand.LandX, 3));
double SP = (UpLand.LandX - NextLand.LandX) * (InflectLand.LandY - NextLand.LandY) - (UpLand.LandY - NextLand.LandY) * (InflectLand.LandX - NextLand.LandX);
if (SP > 0.05)
{
UpLand.sway = SwayEnum.Left;
}
else if (SP < -0.05)
{
UpLand.sway = SwayEnum.Right;
}
else
{
UpLand.sway = SwayEnum.None;
}
}
}
if (Segment != null && Segment.ExcuteMoveDirect != -1)
{
UpLand.movedirect = (MoveDirectEnum)Segment.ExcuteMoveDirect;
}
}
#endregion
#region 计算角度
else
{
//if (Math.Round(UpLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero))
if (UpLand.LandX == InflectLand.LandX)
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
UpLand.Angle = Seg.ExcuteAngle;
}
else
{
if (InflectLand.LandY > UpLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 270;
}
}
}
}
//else if (Math.Round(UpLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero))
else if (UpLand.LandY == InflectLand.LandY)
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
UpLand.Angle = Seg.ExcuteAngle;
}
else
{
if (InflectLand.LandX > UpLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 180;
}
}
}
}
}
#endregion
}
else
{
#region 计算转向
if (IsCountAngel == 0)
{
AllSegment SegmentUPLand = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (SegmentUPLand != null && SegmentUPLand.ExcuteTurnDirect != -1)
{
UpLand.sway = (SwayEnum)SegmentUPLand.ExcuteTurnDirect;
}
else
{
if (AllSegs.Count(p => p.BeginLandMakCode == UpLand.LandmarkCode) > 1)
{
//double SP = (Math.Round(UpLand.LandX, 3) - Math.Round(NextLand.LandX, 3)) * (Math.Round(InflectLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) - (Math.Round(UpLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) * (Math.Round(InflectLand.LandX, 3) - Math.Round(NextLand.LandX, 3));
double SP = (UpLand.LandX - NextLand.LandX) * (InflectLand.LandY - NextLand.LandY) - (UpLand.LandY - NextLand.LandY) * (InflectLand.LandX - NextLand.LandX);
if (SP > 0.05)
{
UpLand.sway = SwayEnum.Left;
}
else if (SP < -0.05)
{
UpLand.sway = SwayEnum.Right;
}
else
{
UpLand.sway = SwayEnum.None;
}
}
}
if (SegmentUPLand != null && SegmentUPLand.ExcuteMoveDirect != -1)
{
UpLand.movedirect = (MoveDirectEnum)SegmentUPLand.ExcuteMoveDirect;
}
AllSegment Segment = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Segment != null && Segment.ExcuteTurnDirect != -1)
{
InflectLand.sway = (SwayEnum)Segment.ExcuteTurnDirect;
}
if (Segment != null && Segment.ExcuteMoveDirect != -1)
{
InflectLand.movedirect = (MoveDirectEnum)Segment.ExcuteMoveDirect;
}
}
#endregion
#region 计算角度
else
{
//if (Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandX, 1, MidpointRounding.AwayFromZero))
if (InflectLand.LandX == NextLand.LandX)
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandY > InflectLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 270;
}
}
}
}
//else if (Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandY, 1, MidpointRounding.AwayFromZero))
else if (InflectLand.LandY == NextLand.LandY)
{
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandX > InflectLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 180;
}
}
}
}
}
#endregion
}
}//end foreach
}
else
{
#region 计算转向
if (IsCountAngel == 0)
{
InflectLand = Routes[0];
NextLand = Routes[1];
AllSegment Segment = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Segment != null && Segment.ExcuteTurnDirect != -1)
{
InflectLand.sway = (SwayEnum)Segment.ExcuteTurnDirect;
}
if (Segment != null && Segment.ExcuteMoveDirect != -1)
{
InflectLand.movedirect = (MoveDirectEnum)Segment.ExcuteMoveDirect;
}
}
#endregion
#region 计算角度
else
{
InflectLand = Routes[0];
NextLand = Routes[1];
//if (Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandX, 1, MidpointRounding.AwayFromZero))
if (InflectLand.LandX == NextLand.LandX)
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandY > InflectLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 270;
}
}
}
}
//else if (Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandY, 1, MidpointRounding.AwayFromZero))
else if (InflectLand.LandY == NextLand.LandY)
{
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandX > InflectLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 180;
}
}
}
}
}
#endregion
}
}
catch (Exception ex)
{ throw ex; }
}
public void Move()
{
try
{
CarActive_Timer.Enabled = false;
if (IsMove)
{
if (CurrRoute.Count() > 0 && ScalingRate > 0)
{
CurrRoute = CurrRoute.Distinct().ToList <LandmarkInfo>();
if (Math.Abs(X - CurrRoute.Last().LandX *ScalingRate) <= 0.035 && Math.Abs(Y - CurrRoute.Last().LandY *ScalingRate) <= 0.035)
{
DateTime dtBegin = DateTime.Now;
CurrLand = CurrRoute.Last();
X = (float)(CurrLand.LandX * ScalingRate);
Y = (float)(CurrLand.LandY * ScalingRate);
NextLand = null;
CurrSite = Convert.ToInt16(CurrLand.LandmarkCode);
Rundistance = 0;
CurrRoute.Clear();
Sate = 0;
CarActive_Timer.Enabled = false;
SendMove();
return;
}
if (Sate == 0)
{
DateTime dtBegin = DateTime.Now;
CurrLand = CurrRoute[0];
Rundistance = 0;
X = (float)(CurrLand.LandX * ScalingRate);
Y = (float)(CurrLand.LandY * ScalingRate);
OldSite = CurrSite;
CurrSite = Convert.ToInt16(CurrLand.LandmarkCode);
int index = CurrRoute.FindIndex(p => p.LandmarkCode == CurrLand.LandmarkCode);
if (index + 1 < CurrRoute.Count)
{
NextLand = CurrRoute[index + 1];
}
Sate = 1;
}
else
{
DateTime dtBegin = DateTime.Now;
LandmarkInfo land = CurrRoute.FirstOrDefault(p => Math.Abs(p.LandX * ScalingRate - X) <= 0.035 && Math.Abs(p.LandY * ScalingRate - Y) <= 0.035);
if (land != null && NextLand != null && land.LandmarkCode == NextLand.LandmarkCode)
{
X = (float)(land.LandX * ScalingRate);
Y = (float)(land.LandY * ScalingRate);
int index = CurrRoute.FindIndex(p => p.LandmarkCode == land.LandmarkCode);
CurrLand = CurrRoute[index];
OldSite = CurrSite;
CurrSite = Convert.ToInt16(CurrLand.LandmarkCode);
if (index + 1 < CurrRoute.Count)
{
NextLand = CurrRoute[index + 1];
}
Rundistance = 0;
Sate = 1;
DateTime dtEnd = DateTime.Now;
}
else
{
DateTime dtBegin1 = DateTime.Now;
if (CurrLand != null && NextLand != null)
{
Rundistance += Speed;
DateTime dtEnd1 = DateTime.Now;
Sate = 1;
}
}
}
//DateTime dtBegin2 = DateTime.Now;
SendMove();
//DateTime dtEnd2 = DateTime.Now;
}
else
{
Sate = 0;
RouteLands.Clear();
TurnLands.Clear();
this.Dispose();
CarActive_Timer.Enabled = false;
}
}
CarActive_Timer.Enabled = true;
}
catch (Exception ex)
{ throw ex; }
//finally
//{CarActive_Timer.Enabled = true;}
}
public void AcountRoute(LandmarkInfo StarLand, LandmarkInfo EndLand)
{
try
{
if (Segments.Count > 0)
{
LandmarkInfo PreLand = StarLand;
if (CloseList.Count < OpenAmount && PreLand.LandmarkCode != EndLand.LandmarkCode)
{
if (CloseList.Where(p => p.LandmarkCode == StarLand.LandmarkCode).Count() <= 0)
{
RouteList.Add(CreateDeepCopy <LandmarkInfo>(StarLand));
CloseList.Add(CreateDeepCopy <LandmarkInfo>(StarLand));
}
List <LandmarkInfo> lands = getNextLand(PreLand);
if (lands.Count == 0)
{
if (PivotAnotherLand.Count > 0)
{
Hashtable hs = PivotAnotherLand.Pop() as Hashtable;
ArrayList keyList = new ArrayList(hs.Keys);
LandmarkInfo PivotLand = hs[keyList[0].ToString()] as LandmarkInfo;
int index = RouteList.FindIndex(p => p.LandmarkCode == keyList[0].ToString()) + 1;
RouteList.RemoveRange(index, RouteList.Count - index);
PreLand = PivotLand;
}
else
{
return;
}
}
else if (lands.Count == 1)
{
if (CloseList.Where(p => p.LandmarkCode == lands[0].LandmarkCode).Count() <= 0)
{
RouteList.Add(CreateDeepCopy <LandmarkInfo>(lands[0]));
CloseList.Add(CreateDeepCopy <LandmarkInfo>(lands[0]));
}
PreLand = lands[0];
}
else
{
List <LandmarkInfo> CloseLand = (from a in lands
orderby getDistant(a.LandX, a.LandY, EndLand.LandX, EndLand.LandY) ascending
select a).ToList();
if (CloseLand == null)
{
throw (new Exception("计算路径异常!"));
}
else
{
if (CloseList.Where(p => p.LandmarkCode == CloseLand[0].LandmarkCode).Count() <= 0)
{
RouteList.Add(CreateDeepCopy <LandmarkInfo>(CloseLand[0]));
CloseList.Add(CreateDeepCopy <LandmarkInfo>(CloseLand[0]));
for (int i = 1; i < CloseLand.Count; i++)
{
Hashtable hs = new Hashtable();
hs[PreLand.LandmarkCode] = CloseLand[i];
PivotAnotherLand.Push(hs);
}
//Hashtable hs = new Hashtable();
//hs[PreLand.LandmarkCode] = CloseLand[1];
//PivotAnotherLand.Push(hs);
}
PreLand = CloseLand[0];
}
}
AcountRoute(PreLand, EndLand);
}
}//end
}
catch (Exception ex)
{ throw ex; }
}
public List <LandmarkInfo> GetRoute(LandmarkInfo StartLand, LandmarkInfo EndLand)
{
try
{
lock (GetRoutObj)
{
#region Old
if (StartLand == null || EndLand == null)
{
return(new List <LandmarkInfo>());
}
RouteList.Clear();
CloseList.Clear();
PivotAnotherLand.Clear();
List <List <LandmarkInfo> > hs_route = new List <List <LandmarkInfo> >();
AcountRoute(StartLand, EndLand);
hs_route.Add(CreateDeepCopy <List <LandmarkInfo> >(RouteList));
List <LandmarkInfo> OrialRoute = CreateDeepCopy <List <LandmarkInfo> >(RouteList);
List <LandmarkInfo> OrialClose = CreateDeepCopy <List <LandmarkInfo> >(CloseList);
Stack NewPivotAnotherLand = CreateDeepCopy <Stack>(PivotAnotherLand);
while (NewPivotAnotherLand.Count > 0)
{
PivotAnotherLand.Clear();
RouteList = CreateDeepCopy <List <LandmarkInfo> >(OrialRoute);
CloseList = CreateDeepCopy <List <LandmarkInfo> >(OrialClose);
Hashtable hs = NewPivotAnotherLand.Pop() as Hashtable;
ArrayList keyList = new ArrayList(hs.Keys);
LandmarkInfo PivotLand = hs[keyList[0].ToString()] as LandmarkInfo;
try
{
int index = RouteList.FindIndex(p => p.LandmarkCode == keyList[0].ToString()) + 1;
int indexClose = CloseList.FindIndex(p => p.LandmarkCode == keyList[0].ToString()) + 1;
if (index >= 0 && index < RouteList.Count)
{
RouteList.RemoveRange(index, RouteList.Count - index);
}
if (indexClose >= 0 && indexClose < CloseList.Count)
{
CloseList.RemoveRange(indexClose, CloseList.Count - indexClose);
}
}
catch (Exception ex)
{ LogHelper.WriteErrorLog(ex); }
AcountRoute(PivotLand, EndLand);
hs_route.Add(CreateDeepCopy <List <LandmarkInfo> >(RouteList));
}
List <LandmarkInfo> Routes = (from a in hs_route
where a.Where(p => p.LandmarkCode == EndLand.LandmarkCode).Count() > 0
orderby a.Count ascending
select a).FirstOrDefault();
if (Routes == null)
{
Routes = new List <LandmarkInfo>();
return(Routes);
}
//AcountDirect(ref Routes);
//return Routes;
RouteList = CreateDeepCopy <List <LandmarkInfo> >(Routes);
#endregion
#region New
//CloseList.Clear();
//RouteList.Clear();
//AcountRoute(StartLand, EndLand);
//RepairRoute();
AcountDirect(ref RouteList);
return(RouteList);
#endregion
}
}
catch (Exception ex)
{ LogHelper.WriteErrorLog(ex); return(new List <LandmarkInfo>()); }
}
//计算转向和前进和后退
public void AcountDirect(ref List <LandmarkInfo> Routes)
{
try
{
IList <AllSegment> AllSegs = AGVClientDAccess.LoadAllSegment();
DataTable dtCoor = AGVClientDAccess.LoadAGVCoordinate();
LandmarkInfo UpLand = null;
LandmarkInfo InflectLand = null;
LandmarkInfo NextLand = null;
if (Routes.Count <= 1)
{
return;
}
else if (Routes.Count >= 3)
{
for (int i = 2; i < Routes.Count; i++)
{
UpLand = Routes[i - 2];
InflectLand = Routes[i - 1];
NextLand = Routes[i];
//判断退进
// var A = Math.Atan2(Math.Round(NextLand.LandY, 3) - Math.Round(InflectLand.LandY, 3), Math.Round(NextLand.LandX, 3) - Math.Round(InflectLand.LandX, 3));
// var B = Math.Atan2(Math.Round(UpLand.LandY, 3) - Math.Round(InflectLand.LandY, 3), Math.Round(UpLand.LandX, 3) - Math.Round(InflectLand.LandX, 3));
// var C = B - A;
// double angel = (180 * (float)C) / Math.PI;
// if (Math.Abs(angel) < 90)
// { InflectLand.IsBack = true; }
double SP = (Math.Round(UpLand.LandX, 3) - Math.Round(NextLand.LandX, 3)) * (Math.Round(InflectLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) - (Math.Round(UpLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) * (Math.Round(InflectLand.LandX, 3) - Math.Round(NextLand.LandX, 3));
if (SP > 0.05)
{
InflectLand.sway = SwayEnum.Left;
}
else if (SP < -0.05)
{
InflectLand.sway = SwayEnum.Right;
}
else
{
InflectLand.sway = SwayEnum.None;
}
//判断第一个地标角度
if (i == 2)//计算开始行走的角度
{
if (Math.Round(UpLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero))
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
UpLand.Angle = Seg.ExcuteAngle;
}
else
{
if (InflectLand.LandY > UpLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 270;
}
}
}
}
else if (Math.Round(UpLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero))
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
UpLand.Angle = Seg.ExcuteAngle;
}
else
{
if (InflectLand.LandX > UpLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 180;
}
}
}
}
}
//判断最后一个地标的角度
if (i == Routes.Count - 1)//计算开始行走的角度
{
NextLand.Angle = InflectLand.Angle;
}
if (SP > 0.05 || SP < -0.05)
{
if (Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandX, 1, MidpointRounding.AwayFromZero))
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandY > InflectLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 270;
}
}
}
}
else if (Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandY, 1, MidpointRounding.AwayFromZero))
{
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandX > InflectLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 180;
}
}
}
}
}
}//end foreach
}
else
{
InflectLand = Routes[0];
NextLand = Routes[1];
if (Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandX, 1, MidpointRounding.AwayFromZero))
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandY > InflectLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 270;
}
}
}
}
else if (Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandY, 1, MidpointRounding.AwayFromZero))
{
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandX > InflectLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 180;
}
}
}
}
}
}
catch (Exception ex)
{ throw ex; }
}
public List <LandmarkInfo> GetRoute(LandmarkInfo StartLand, LandmarkInfo EndLand)
{
try
{
List <List <LandmarkInfo> > hs_route = new List <List <LandmarkInfo> >();
AcountRoute(StartLand, EndLand);
hs_route.Add(CreateDeepCopy <List <LandmarkInfo> >(RouteList));
List <LandmarkInfo> OrialRoute = CreateDeepCopy <List <LandmarkInfo> >(RouteList);
List <LandmarkInfo> OrialClose = CreateDeepCopy <List <LandmarkInfo> >(CloseList);
Stack NewPivotAnotherLand = CreateDeepCopy <Stack>(PivotAnotherLand);
while (NewPivotAnotherLand.Count > 0)
{
PivotAnotherLand.Clear();
RouteList = CreateDeepCopy <List <LandmarkInfo> >(OrialRoute);
CloseList = CreateDeepCopy <List <LandmarkInfo> >(OrialClose);
Hashtable hs = NewPivotAnotherLand.Pop() as Hashtable;
ArrayList keyList = new ArrayList(hs.Keys);
LandmarkInfo PivotLand = hs[keyList[0].ToString()] as LandmarkInfo;
int index = RouteList.FindIndex(p => p.LandmarkCode == keyList[0].ToString()) + 1;
int indexClose = CloseList.FindIndex(p => p.LandmarkCode == keyList[0].ToString()) + 1;
RouteList.RemoveRange(index, RouteList.Count - index);
CloseList.RemoveRange(indexClose, CloseList.Count - indexClose);
AcountRoute(PivotLand, EndLand);
hs_route.Add(CreateDeepCopy <List <LandmarkInfo> >(RouteList));
}
List <LandmarkInfo> Routes = null;
SysParameter sys = AGVClientDAccess.GetParameterByCode("RouteCountMode");
if (sys != null)
{
if (sys.ParameterValue == "最少地标数")
{
Routes = (from a in hs_route
where a.Where(p => p.LandmarkCode == EndLand.LandmarkCode).Count() > 0
orderby a.Count ascending
select a).FirstOrDefault();
}
else if (sys.ParameterValue == "最少拐弯")
{
Routes = (from a in hs_route
where a.Where(p => p.LandmarkCode == EndLand.LandmarkCode).Count() > 0
orderby a.Count ascending
select a).FirstOrDefault();
Routes = (from a in hs_route
where a.Where(p => p.LandmarkCode == EndLand.LandmarkCode).Count() > 0
orderby AcountInflect(a) ascending
select a).FirstOrDefault();
}
}
else
{
Routes = (from a in hs_route
where a.Where(p => p.LandmarkCode == EndLand.LandmarkCode).Count() > 0
orderby a.Count ascending
select a).FirstOrDefault();
}
if (Routes == null)
{
Routes = new List <LandmarkInfo>();
}
//this.RouteList = Routes;
AcountDirect(ref Routes);
return(Routes);
}
catch (Exception ex)
{ throw ex; }
}
/// <summary>
/// 分派任务
/// </summary>
private async void assignTask()
{
try
{
lock (HandleTaskobje)
{
timerFreshTask.Enabled = false;
foreach (CarMonitor car in MoniCars.Where(p => (!string.IsNullOrEmpty(p.ExcuteTaksNo)) && p.TaskDetailID >= 0))
{
//将当前的任务明细更新完成
if (!string.IsNullOrEmpty(car.ExcuteTaksNo) && car.TaskDetailID != -1)
{
//HandTaskDetail(car.CurrSite.ToString(), car.ExcuteTaksNo, car.TaskDetailID);
if (car.Sate == 0 && car.CurrSite.ToString() == car.ArmLand)
{
#region 更新储位状态
UnLockStorage(car, Stores);
#endregion
AGVSimulationDAccess.HandTaskDetail(car.AgvID, car.CurrSite.ToString(), car.ExcuteTaksNo, car.TaskDetailID);
AGVSimulationDAccess.HandTaskzb(car.ExcuteTaksNo);
DispatchTaskInfo CarTaskInfo = AGVSimulationDAccess.CheckTaskIsFinish(car.ExcuteTaksNo);
if (CarTaskInfo == null)
{
car.ExcuteTaksNo = "";
}
}
}
}
Hashtable hs = new Hashtable();
//查找当前有效任务
IList <DispatchTaskInfo> tasks = AGVSimulationDAccess.LoadDispatchTask("0,1");
if (tasks != null && tasks.Count > 0)
{
foreach (DispatchTaskInfo task in tasks)
{
if (task.TaskDetail.Count <= 0)
{
continue;
}
//查找合适的AGV来领取任务
CarMonitor NoTaskCar = null;
//如果任务为待处理的,就找空闲的车来执行
//如果任务是正在执行中的，则找对应的空闲agv来继续执行
//查找任务到具体任务明细
DispatchTaskDetail TaskDetail = (from a in task.TaskDetail
where (a.State == 0 || a.State == 1)
orderby a.DetailID ascending
select a).FirstOrDefault();
if (task.TaskState == 0)
{
if (TaskDetail != null)
{
if (task.ExeAgvID != 0)
{
NoTaskCar = (from a in MoniCars
where task.ExeAgvID == a.AgvID && !string.IsNullOrEmpty(a.CurrSite.ToString()) && (a.Sate == 0 || a.IsBack) && a.ExcuteTaksNo == "" && /*a.TaskDetailID == -1*/
getDistant(a.CurrSite.ToString(), TaskDetail.LandCode) >= 0
orderby getDistant(a.CurrSite.ToString(), TaskDetail.LandCode) ascending
select a).FirstOrDefault();
}
else
{
NoTaskCar = (from a in MoniCars
where !string.IsNullOrEmpty(a.CurrSite.ToString()) && (a.Sate == 0 || a.IsBack) && a.ExcuteTaksNo == "" && /*a.TaskDetailID == -1*/
getDistant(a.CurrSite.ToString(), TaskDetail.LandCode) >= 0
orderby getDistant(a.CurrSite.ToString(), TaskDetail.LandCode) ascending
select a).FirstOrDefault();
}
}
}
else if (task.TaskState == 1)
{
NoTaskCar = MoniCars.FirstOrDefault(p => p.AgvID == task.ExeAgvID && ((p.Sate == 0 || p.Sate == 2 || p.IsBack)));
}
//找到对应的agv
if (NoTaskCar != null && NoTaskCar.CurrSite > 0)
{
if (NoTaskCar.IsBack)
{
NoTaskCar.IsBack = !NoTaskCar.IsBack;
}
}
else
{
continue;
}
//如果任务明细全部完成，那么需要将主任务状态更新并且让对应的agv回待命点
LandmarkInfo BeginLand, EndLand = null;
if (TaskDetail == null)
{
}
else
{
//判断一下当前小时是否就在任务地标上
if (NoTaskCar.CurrSite.ToString() == TaskDetail.LandCode && (TaskDetail.State == 0 || TaskDetail.State == 1))
{
NoTaskCar.OperType = TaskDetail.OperType;
NoTaskCar.PutType = TaskDetail.PutType;
NoTaskCar.ArmLand = TaskDetail.LandCode;
NoTaskCar.ExcuteTaksNo = TaskDetail.dispatchNo;
NoTaskCar.TaskDetailID = TaskDetail.DetailID;
AGVSimulationDAccess.HandTaskDetail(NoTaskCar.AgvID, TaskDetail.LandCode, TaskDetail.dispatchNo, TaskDetail.DetailID);
UnLockStorage(NoTaskCar, Stores);
//NoTaskCar.TaskDetailID = -1;
continue;
}
//判断小车当前的任务明细是否允许需要等待放行命令
DispatchTaskDetail PreTaskDetail = (from a in task.TaskDetail
where a.LandCode == NoTaskCar.CurrSite.ToString()
orderby a.DetailID ascending
select a).FirstOrDefault();
//如果不允许执行则跳过
if (PreTaskDetail != null && PreTaskDetail.IsAllowExcute == 0)
{
continue;
}
//记录当前小车的任务信息
//更新任务为执行状态
AGVSimulationDAccess.TaskHandle(task.dispatchNo, NoTaskCar.AgvID, 1, TaskDetail.LandCode, TaskDetail.DetailID);
NoTaskCar.ExcuteTaksNo = task.dispatchNo;
NoTaskCar.TaskDetailID = TaskDetail.DetailID;
NoTaskCar.ArmLand = TaskDetail.LandCode;
NoTaskCar.PutType = TaskDetail.PutType;
NoTaskCar.OperType = TaskDetail.OperType;
BeginLand = AllLands.FirstOrDefault(p => p.LandmarkCode == NoTaskCar.CurrSite.ToString());
EndLand = AllLands.FirstOrDefault(p => p.LandmarkCode == TaskDetail.LandCode);
if (BeginLand != null && EndLand != null)
{
NoTaskCar.CurrRoute = DataToObject.CreateDeepCopy <List <LandmarkInfo> >(CountRoute.GetRoute(BeginLand, EndLand));
string CountRouteResult = string.Join(",", NoTaskCar.CurrRoute.Select(p => p.LandmarkCode));
LogHelper.WriteLog(CountRouteResult);
if (NoTaskCar.CurrRoute.Count <= 0)
{
continue;
}
if (!Traffic.BeforStartTrafficForStop(NoTaskCar))
{
Traffic.GetTrafficResour(NoTaskCar);
//if (Route_Ini != null)
//{ Route_Ini(NoTaskCar.CurrRoute); }
NoTaskCar.StepChange += DoCar_StepChange;
NoTaskCar.Start();
}
}
}
}
}
else
{
List <CarMonitor> NoReleaseCars = MoniCars.Where(p => p.Sate != 1 && p.Sate != 2 && !string.IsNullOrEmpty(p.ExcuteTaksNo)).ToList();
foreach (CarMonitor item in NoReleaseCars)
{
item.ExcuteTaksNo = "";
}
}
List <CarMonitor> NoBackCars = MoniCars.Where(p => p.Sate != 1 && p.Sate != 2 && string.IsNullOrEmpty(p.ExcuteTaksNo) && p.CurrSite.ToString() != p.StandbyLandMark).ToList();
foreach (CarMonitor car in NoBackCars)
{
if (string.IsNullOrEmpty(car.StandbyLandMark))
{
continue;
}
LandmarkInfo BeginLand = AllLands.FirstOrDefault(p => p.LandmarkCode == car.CurrSite.ToString());
LandmarkInfo EndLand = AllLands.FirstOrDefault(p => p.LandmarkCode == car.StandbyLandMark);
//以车的当前地标为开始地标,待命点为结束地标规划回待命点路线
if (BeginLand == null || EndLand == null)
{
continue;
}
else
{
//规划路线
car.CurrRoute = DataToObject.CreateDeepCopy <List <LandmarkInfo> >(CountRoute.GetRoute(BeginLand, EndLand));
if (car.CurrRoute.Count <= 0)
{
continue;
}
if (!Traffic.BeforStartTrafficForStop(car))
{
Traffic.GetTrafficResour(car);
//if (Route_Ini != null)
//{ Route_Ini(car.CurrRoute); }
car.StepChange += DoCar_StepChange;
car.Start();
}
}
}
}
}
catch (Exception ex)
{ throw ex; }
finally
{ timerFreshTask.Enabled = true; }
}
/// <summary>
/// SendPackIndex 为发送包序号
/// 命令反馈
/// </summary>
public bool GetCallBack(Int16 SendPackIndex = 0, bool IsCheckCarState = false)
{
try
{
int AGVID = this.DeviceID;
int offlinecount = 0;
int allheadleftlengh = 7;
int receivedlengh = 0;
byte[] bufferhead = new byte[7];//定义6位长度为接收包头缓存长度
while (allheadleftlengh - receivedlengh > 0)
{
byte[] buffertemp = new byte[allheadleftlengh - receivedlengh];
if (Tcpsocket.Available <= 0)
{
continue;
}
int lengh = Tcpsocket.Receive(buffertemp);
if (lengh <= 0)
{
if (offlinecount == 3)
{
LogHelper.WriteReciveAGVMessLog("接受的小车" + AGVID.ToString() + "反馈命令超时");
return(false);
}
offlinecount += 1;
Thread.Sleep(50);
}
Buffer.BlockCopy(buffertemp, 0, bufferhead, receivedlengh, lengh);
receivedlengh += lengh;
}
//解析AGV反馈信息
if (bufferhead[0] == 0xEB && bufferhead[1] == 0x90 && bufferhead[2] == 0XD1)
{
//判断一下报文序号
byte[] arr = new byte[] { bufferhead[4], bufferhead[3] };
Int16 RecvPackIndex = BitConverter.ToInt16(arr, 0);
if (SendPackIndex == 0 || (SendPackIndex != 0 && RecvPackIndex == SendPackIndex))//如果发送和接受报文序号一致,说明成功
{
offlinecount = 0;
byte[] bytess = new byte[2] {
bufferhead[6], bufferhead[5]
};
//int allcontentleftlengh = bufferhead[6] << 8 + bufferhead[5];
int allcontentleftlengh = BitConverter.ToInt16(bytess, 0) + 1;
receivedlengh = 0;
byte[] buffercontent = new byte[allcontentleftlengh];
while (allcontentleftlengh - receivedlengh > 0)
{
byte[] buffertemp = new byte[allcontentleftlengh - receivedlengh];
if (Tcpsocket.Available <= 0)
{
continue;
}
int lengh = Tcpsocket.Receive(buffertemp);
if (lengh <= 0)
{
if (offlinecount == 3)
{
LogHelper.WriteReciveAGVMessLog("接受的小车" + AGVID.ToString() + "反馈命令超时");
return(false);
}
offlinecount += 1;
Thread.Sleep(50);
}
Buffer.BlockCopy(buffertemp, 0, buffercontent, receivedlengh, lengh);
receivedlengh += lengh;
}
//接受完内容后校验
List <byte> msg = new List <byte>();
msg.AddRange(bufferhead);
msg.AddRange(buffercontent);
string SenDLog = "";
foreach (byte item in msg)
{
SenDLog += ((int)item).ToString("X") + " ";
}
LogHelper.WriteReciveAGVMessLog("接受的小车" + AGVID.ToString() + "反馈命令:" + SenDLog);
byte CRC8 = CRC.CRC8(msg.Take(allcontentleftlengh + 6).ToArray());
if (CRC8 != msg[msg.Count - 1])
{
LogHelper.WriteReciveAGVMessLog("接受的小车" + AGVID.ToString() + "反馈命令校验位不正确");
return(false);
}
//解析车辆信息
CarInfo car = new CarInfo();
car.AgvID = this.DeviceID;
car.CarState = Convert.ToInt32(msg[7]);
car.IsNeedFinshTask = Convert.ToInt32(msg[8]);
car.IsNeedRedoTask = Convert.ToInt32(msg[9]);
byte[] bytes = new byte[4] {
msg[13], msg[12], msg[11], msg[10]
};
car.X = BitConverter.ToInt32(bytes, 0) / 1000.00F;
bytes = new byte[4] {
msg[17], msg[16], msg[15], msg[14]
};
car.Y = BitConverter.ToInt32(bytes, 0) / 1000.00F;
bytes = new byte[4] {
msg[21], msg[20], msg[19], msg[18]
};
car.Angel = BitConverter.ToInt32(bytes, 0) / 1000.00F;
//是否在码上
car.IsUpLand = Convert.ToInt32(msg[22].ToString());
//升降平台的状态
car.BangState = Convert.ToInt32(msg[23].ToString());
//夹持机构状态
car.JCState = Convert.ToInt32(msg[24].ToString());
//剩余电池容量 百分比
car.fVolt = Convert.ToInt32(msg[25].ToString());
//速度
car.speed = Convert.ToInt32(msg[26].ToString());
//报警长度
int WarnLen = Convert.ToInt32(msg[27].ToString());
car.ErrorMessage = "";
if (WarnLen > 0)//有报警
{
int StartIndex = 27;
for (int i = 1; i <= WarnLen; i++)
{
int Cat = StartIndex + 1;
int Code = StartIndex + 2;
car.WarnType = Convert.ToInt32(msg[Cat].ToString());
car.WarnBinaryCode = Convert.ToString(msg[Code], 2).PadLeft(8, '0');
switch (car.WarnType)
{
case 0:
if (car.WarnBinaryCode.Substring(7, 1) == "1")
{
car.ErrorMessage += "24C02错误-写入错误!\r\n";
}
if (car.WarnBinaryCode.Substring(6, 1) == "1")
{
car.ErrorMessage += "24C02错误-读取错误\r\n";
}
break;
case 1:
if (car.WarnBinaryCode.Substring(7, 1) == "1")
{
car.ErrorMessage += "电量检测错误-通信错误\r\n";
}
if (car.WarnBinaryCode.Substring(6, 1) == "1")
{
car.ErrorMessage += "电量检测错误-电量低\r\n";
}
break;
case 2:
if (car.WarnBinaryCode.Substring(7, 1) == "1")
{
car.ErrorMessage += "WiFi通信错误-校验错误\r\n";
}
if (car.WarnBinaryCode.Substring(6, 1) == "1")
{
car.ErrorMessage += "WiFi通信错误-心跳超时\r\n";
}
break;
case 3:
if (car.WarnBinaryCode.Substring(7, 1) == "1")
{
car.ErrorMessage += "433M通信错误\r\n";
}
if (car.WarnBinaryCode.Substring(6, 1) == "1")
{
car.ErrorMessage += "校验错误\r\n";
}
break;
case 4:
if (car.WarnBinaryCode.Substring(7, 1) == "1")
{
car.ErrorMessage += "PGV100错误-校验错误1\r\n";
}
if (car.WarnBinaryCode.Substring(6, 1) == "1")
{
car.ErrorMessage += "PGV100错误-通信超时1\r\n";
}
if (car.WarnBinaryCode.Substring(5, 1) == "1")
{
car.ErrorMessage += "PGV100错误-硬件故障1\r\n";
}
if (car.WarnBinaryCode.Substring(4, 1) == "1")
{
car.ErrorMessage += "PGV100错误-校验错误2\r\n";
}
if (car.WarnBinaryCode.Substring(3, 1) == "1")
{
car.ErrorMessage += "PGV100错误-通信超时2\r\n";
}
if (car.WarnBinaryCode.Substring(2, 1) == "1")
{
car.ErrorMessage += "PGV100错误-硬件故障2\r\n";
}
break;
case 5:
if (car.WarnBinaryCode.Substring(7, 1) == "1")
{
car.ErrorMessage += "驱动器-1号通信异常\r\n";
}
if (car.WarnBinaryCode.Substring(6, 1) == "1")
{
car.ErrorMessage += "驱动器-2号通信异常\r\n";
}
if (car.WarnBinaryCode.Substring(5, 1) == "1")
{
car.ErrorMessage += "驱动器-3号通信异常\r\n";
}
if (car.WarnBinaryCode.Substring(4, 1) == "1")
{
car.ErrorMessage += "驱动器-4号通信异常\r\n";
}
if (car.WarnBinaryCode.Substring(3, 1) == "1")
{
car.ErrorMessage += "驱动器-1号硬件故障\r\n";
}
if (car.WarnBinaryCode.Substring(2, 1) == "1")
{
car.ErrorMessage += "驱动器-2号硬件故障\r\n";
}
if (car.WarnBinaryCode.Substring(1, 1) == "1")
{
car.ErrorMessage += "驱动器-3号硬件故障\r\n";
}
if (car.WarnBinaryCode.Substring(0, 1) == "1")
{
car.ErrorMessage += "驱动器-3号硬件故障\r\n";
}
break;
case 6:
if (car.WarnBinaryCode.Substring(7, 1) == "1")
{
car.ErrorMessage += "安全传感器-触边\r\n";
}
if (car.WarnBinaryCode.Substring(6, 1) == "1")
{
car.ErrorMessage += "安全传感器-激光减速\r\n";
}
if (car.WarnBinaryCode.Substring(5, 1) == "1")
{
car.ErrorMessage += "安全传感器-激光停车\r\n";
}
if (car.WarnBinaryCode.Substring(4, 1) == "1")
{
car.ErrorMessage += "安全传感器-急停按钮\r\n";
}
break;
case 7:
if (car.WarnBinaryCode.Substring(7, 1) == "1")
{
car.ErrorMessage += "SIM2000-校验错误\r\n";
}
if (car.WarnBinaryCode.Substring(6, 1) == "1")
{
car.ErrorMessage += "SIM2000-通信超时\r\n";
}
if (car.WarnBinaryCode.Substring(5, 1) == "1")
{
car.ErrorMessage += "SIM2000-硬件故障\r\n";
}
break;
case 8:
if (car.WarnBinaryCode.Substring(7, 1) == "1")
{
car.ErrorMessage += "升降平台-未下降到位\r\n";
}
if (car.WarnBinaryCode.Substring(6, 1) == "1")
{
car.ErrorMessage += "升降平台-未上升到位\r\n";
}
break;
case 9:
if (car.WarnBinaryCode.Substring(7, 1) == "1")
{
car.ErrorMessage += "夹持机构-未夹紧到位\r\n";
}
if (car.WarnBinaryCode.Substring(6, 1) == "1")
{
car.ErrorMessage += "夹持机构-未张开到位\r\n";
}
break;
case 10:
if (car.WarnBinaryCode.Substring(7, 1) == "1")
{
car.ErrorMessage += "导航-丢失反光柱匹配\r\n";
}
if (car.WarnBinaryCode.Substring(6, 1) == "1")
{
car.ErrorMessage += "导航-丢失定位二维码\r\n";
}
if (car.WarnBinaryCode.Substring(5, 1) == "1")
{
car.ErrorMessage += "导航-丢失修正二维码\r\n";
}
if (car.WarnBinaryCode.Substring(4, 1) == "1")
{
car.ErrorMessage += "导航-目标点对比异常\r\n";
}
break;
default:
break;
}
StartIndex += 2;
}
}
if (!string.IsNullOrEmpty(car.ErrorMessage))
{
LogHelper.WriteAGVWarnMessLog(car.AgvID.ToString() + "号AGV报警异常信息:" + car.ErrorMessage);
}
LandmarkInfo CurrLand = CoreData.AllLands.FirstOrDefault(p => distance((float)p.LandX, (float)p.LandY, car.X, car.Y) <= 0.2);
if (CurrLand != null)
{
car.CurrSite = Convert.ToInt16(CurrLand.LandmarkCode);
LogHelper.WriteReciveAGVMessLog("小车" + AGVID.ToString() + "坐标得到的地标为:" + CurrLand.LandmarkCode);
car.IsUpQCode = 1;
}
else
{
car.IsUpQCode = 0;
}
DelegateState.InvokeCarFeedbackEvent(car);
LastRecTime = DateTime.Now;
//发送复位需要判断车子状态是否为待命状态
if (IsCheckCarState)
{
LogHelper.WriteSendAGVMessLog("判断发送复位指令是否真正完成!");
if (car.CarState != 0)
{
LogHelper.WriteSendAGVMessLog("发送复位指令车子:" + car.AgvID.ToString() + "状态为:" + car.CarState.ToString());
return(false);
}
}
return(true);
}
else
{
return(false);
}
}
}
catch (Exception ex)
{ LogHelper.WriteLog(this.DeviceID.ToString() + "车 AGV解析编解码错误!" + ex.Message); }
return(false);
}
