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