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